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Extended  Kalman  filtering  is  used  to  provide  estimates  of  the  position  and  velocity 
of  a  target  based  upon  observations  of  the  target’s  bearing  and  range.  Non-stationary 
noise  is  shown  to  degrade  the  performance  of  the  filter  and  cause  filter  divergence.  By 
estimating  the  noise  power  from  the  variance  of  the  filter’s  residual  we  adapt  the  filter 
to  compensate  for  varying  noise  power.  We  also  introduce  the  method  of  correlated 
maneuver  gating  to  adapt  the  Kalman  filter  to  target  dynamics.  By  spatially  and 
temporally  correlating  the  Mahalanobis  Distance  of  the  residual,  the  Kalman  filter’s 
performance  is  increased  while  tracking  tangentially  accelerating  targets.  Monte  Carlo 
simulations  are  run  for  three  different  sets  of  target  dynamics:  stationary,  moving 
*  linearly,  and  accelerating  tangentially.  Results  for  the  simulations  show  significant 
performance  advantages  of  using  correlated  maneuver  gating  in  conjunction  with  noise 
adaptation.  These  results  should  generalize  to  other  applications  of  the  extended  Kalman 
filter  whose  state  and  observation  spaces  enjoy  a  one-to-one  mapping. 
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I.  INTRODUCTION 


A.  TARGET  TRACKING 

The  detection,  location,  and  tracking  of  targets  plays  a  critical  role  in  many 
aspects  of  military  operations  as  well  as  civilian  aviation.  The  ability  to  accurately  track 
and  predict  the  movement  of  aircraft,  for  example,  makes  modern  air-traffic-control 
possible,  and  the  importance  of  target  tracking  in  military  operations  can  not  be 
overstated.  This  report  investigates  a  particularly  powerful  method  of  target  tracking 
'known  as  Kalman  filtering. 

Kalman  filtering  has  been  used  with  tremendous  success  in  target  tracking  for 
almost  thirty  years.  Its  strength  lies  in  its  firm  mathematical  foundation  of  statistical 
optimality.  We  investigate  the  behavior  of  an  extended  Kalman  filter  in  tracking  a 
target  by  means  of  bearing  and  range  observations.  This  is  the  case  for  most  civilian 
and  military  radars. 

B.  OBJECTIVES  OF  THIS  REPORT 

This  report  treats  the  extended  Kalman  filter  as  a  starting  point,  not  a  destination. 
By  combining  statistics  and  heuristics,  we  introduce  methods  which  can  significantly 
improve  the  performance  of  the  extended  Kalman  filter.  These  methods  are  sufficiently 
general  to  apply  to  a  wide  range  of  applications,  not  just  radar  target  tracking.  The 
methods  proposed  serve  to  allow  the  Kalman  filter  to  adapt  to  its  environment  in  the 
face  of  dynamic  noise  statistics  and  maneuvering  targets. 
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We  begin  by  showing  that  the  Kalman  filter  is  only  optimal  when  it  is  given 
correct  a  priori  information  about  the  noise  sources  affecting  the  system.  The 
performance  of  the  filter  degrades  as  the  noise  power  varies  from  the  a  priori  estimates. 
We  propose  a  method  to  improve  the  performance  of  the  Kalman  filter  in  the  presence 
of  unpredicted  or  non-stationary  noise  processes.  This  method  uses  the  residual  (or 
innovations)  provided  by  the  Kalman  filter  to  estimate  the  statistical  properties  of  the 
noise.  These  parameters  are  then  used  adaptively  in  the  Kalman  equations. 

We  also  show  that  the  ability  of  a  Kalman  filter  to  track  a  tangentially  accelerating 
target  is  improved  greatly  by  maneuver  gating.  Although  maneuver  gating  has  been 
discussed  before  in  the  literature,  we  propose  gating  off  of  the  Mahalanobis  Distance  of 
the  residual.  This  gating  statistic  is  then  processed  for  spatial  and  temporal  correlation. 
It  is  this  correlated  gating  process  which  provides  increased  accuracy  and  divergence 
rejection  to  the  Kalman  filter. 

C.  REPORT  ORGANIZATION 

This  report  is  organized  into  six  major  sections.  The  first  is  this  introduction, 
which  serves  as  a  guide  to  approaching  this  report.  Chapters  II  and  III  give  a 
mathematical  derivation  of  the  extended  Kalman  filter  equations.  Chapter  IV  models  the 
physical  tracking  system  which  is  used  as  the  basis  for  the  simulations  throughout  this 
report.  Chapters  V  and  VI  describe  the  methods  which  we  propose  for  improving  the 
performance  of  the  extended  Kalman  filter.  Chapters  VII  through  IX  show  the 
simulations  and  give  our  results  and  conclusions.  The  appendices  give  supportive 
mathematical  treatments  and  program  code. 

It  is  recommended  that  the  chapters  be  taken  in  the  order  they  are  presented.  The 
reader  who  is  conversive  with  Kalman  filtering,  however,  should  skim  Chapters  II 


through  IV  and  proceed  quickly  to  Chapter  V.  Appendices  A  and  B  give  mathematical 
treatment  to  topics  for  which  we  were  unable  to  find  suitable  derivations  in  the 
literature.  These  appendices  should  prove  useful  for  other  researchers  interested  in  this 
area,  but  are  not  required  to  appreciate  the  results  of  this  research.  Finally,  the  program 
code  listed  in  Appendix  C  is  purely  for  the  benefit  of  others  who  are  conducting  work 
in  target  tracking. 
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II.  KALMAN  FILTER 


The  Kalman  filter  has  been  used  extensively  in  the  design  of  estimation  systems 
since  it  was  first  presented  by  Kalman  and  Bucy  [Ref.  1-2]  in  1960.  Its  rapid 
acceptance  and  subsequent  success  are  due,  in  part,  to  the  Kalman  filter’s  recursive 
na'ture  and  provable  optimality  for  certain  systems.  Also,  the  Kalman  filter  has  the 
desirable  quality  of  maintaining  the  physical  meaning  of  the  system  dynamics  by 
utilizing  a  state  space  representation. 

The  provable  optimality  mentioned  abo‘>e  holds  for  systems  which  are  linear  and 
time-invariant  (LTI),  and  corrupted  by  additive  white  Gaussian  noise  (AWGN).  This 
chapter  will  develop  the  Kalman  filter  equations  for  just  such  a  system.  The 
development  will  follow  closely  that  given  by  Gelb  in  Chapter  IV  of  Ref.  3  and  starts  by 
defining  the  terms  involved  in  the  derivation.  After  that,  we  model  the  physical  system 
and  the  noise  processes  involved.  Then  the  form  of  the  filter  will  be  argued,  rather  than 
proved.  Finally,  after  choosing  criteria  for  optimality,  the  equations  which  optimize  the 
performance  of  this  filter  will  be  derived. 

A.  DEFINITION  OF  TERMS 

All  of  the  terms  used  in  this  chapter  are  defined  in  Table  (2.1).  These  terms  are 
explained  further  when  they  are  introduced  in  the  derivation.  Terms  with  a  single  time 
subscript  (i.e.,  Xk)  refer  to  the  value  of  the  term  at  that  time.  Those  terms  with  dual 
subscripts  (i.e.,  Pk+i|k)  refer  to  the  value  of  the  term  at  the  time  of  the  first  subscript 
given  observations  through  the  second.  Since  no  special  annotation  is  used  to  denote  a 
matrix,  the  third  column  gives  the  dimension  of  each  entry  for  its  matrix  form.  The 
dimensions  are  given  first  in  the  table. 
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TABLE  2.1:  DEFINITION  OF  TERMS 


System  order: 

n 

Observation  size: 

m 

Identity  matrix 

I 

n  x  n 

System  state: 

xk 

n  x  1 

Transpose  of  x: 

„T 
x  k 

1  x  n 

State  transition  matrix: 

* 

n  x  n 

State  excitation  noise: 

wk 

n  x  1 

Observation: 

zk 

m  x  1 

Observation  matrix: 

H 

m  x  n 

Observation  noise: 

vk 

m  x  1 

State  estimate: 

*k+llk 

n  x  1 

Estimate  error: 

*k+llk 

n  x  1 

Expected  value  of  the  error: 

Et^k+llk! 

n  x  1 

Error  covariance  matrix: 

Pk-t-llk+l 

n  x  n 

Residual: 

rk+l 

m  x  1 

B.  SYSTEM  MODEL 

In  order  to  estimate  the  parameters  of  a  system,  it  is  necessary  to  have  a  good 
model  of  that  system.  The  state  space  model  of  our  linear  system  is  given  in  Equation 
(2.1),  and  the  measurements  are  described  by  Equation  (2.2).  This  is  a  standard  state 
space  matrix  representation  for  a  system  of  linear,  discrete-difference  equations. 


=  +  wk 

(2.1) 

Hxk+1  +  Vl 

(2.2) 
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The  physical  state  of  the  system  (position,  velocity,  etc.)  is  described  by  Xk,  and  the 
observed  parameters  (bearing,  range,  etc.)  are  contained  in  Zk. 

This  system  is  time-invariant  because  neither  <j>  nor  H  is  dependent  on  the  time 
subscript  k.  The  noise  processes  are  assumed  to  be  stationary,  independent,  zero-mean, 
AWGN.  Although  this  is  only  an  idealization,  it  is  often  justified  in  real  systems.  The 
statistical  properties  of  the  noise  processes  are  given  in  Equations  (2.3)  through  (2.7). 


E[wk]  -  0  (2.3) 

E£  wjWTk]  -  Qfijk  (2.4) 

E[vk]  -  0  (2.5) 

E[VjvTk3  -  R6jk  (2.6) 

E(WjVTk]  «  0  (2.7) 

The  Kronecker  delta  function,  S,  is  defined  by 

jjk-{o  j;iO-  <2-8> 


The  matrices  Q  and  R  in  Equations  (2.4)  and  (2.6)  are  the  covariance  matrices  for 
the  noise  processes.  For  this  system,  the  noise  covariance  matrices  are  non-zero  diagonal 
matrices  which  denote  the  power  present  in  the  noise.  This  simple,  linear  model  will  be 
generalized  in  Chapter  3. 

C.  LINEAR,  RECURSIVE  FORM 

Before  deriving  the  Kalman  filter  equations  we  will  first  determine  the  form  of 
the  filter.  The  form  we  assume  is  that  of  a  two-step,  linear,  recursive  filter,  as  shown 
in  Equations  (2.9)  and  (2.10).  This  form  is  both  simple  and  efficient.  The  current 
estimate  is  a  linear  combination  of  the  previous  estimate  and  the  current  observation. 
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Since  the  filter  does  not  store  the  observations,  it  requires  only  a  fixed  amount  of 
memory  to  process  an  arbitrary  number  of  observations. 

*k+llk  “  KAik 

*k+llk+l  “  KSxk+llk  +  K3zk+1  (2.10) 

Although  this  form  is  assumed  for  its  simplicity,  Ref.  1  demonstrates  that  it  is  optimal 
for  a  linear  system. 

D.  MEASURE  OF  OPTIMALITY 

An  optimal  system  is  generally  considered  to  be  any  system  which  minimizes  a  cost 
function  (or  maximizes  a  performance  function)  subject  to  specific  constraints.  For  our 
system,  we  desire  a  filter  which  gives  unbiased  estimates  that  minimize  a  function  of  the 
estimate  error.  Constants  Ki  in  Equation  (2.9)  and  K.2  in  Equation  (2.10)  are  chosen  to 
make  the  estimate  unbiased.  The  constant  K3  is  chosen  to  minimize  a  cost  function 
related  to  the  expected  error, 

An  unbiased  estimate  is  an  estimate  whose  error  has  an  expected  value  of  zero. 
This  is  an  entirely  reasonable  desire  for  a  filter  which  we  intend  to  call  optimal.  The 
estimate  errors  are  given  as 

xk+llk  *  xk+l  "  xk+llk  (2.11) 

and 

xk+llk+l  *  xk+l  *  xk+llk+l  (2.12) 

and  their  expected  values  satisfy 

^k+llk+l]  “  EA+lIkJ  =  0-  (2.13) 
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The  value  of  Ki  can  be  determined  by  substituting  Equations  (2.1)  and  (2.9)  into 
Equation  (2.11)  and  then  adding  another  term  to  both  sides  to  yield 

*k+iik  -  #kik  *  K  *  wk  “  *Aik-  #kik-  (2*14> 

By  regrouping  terms  and  takiaa  the  expected  value  of  both  sides,  Equation  (2.14)  can  be 
written  in  a  form  which  can  make  use  of  Equation  (2.13). 

E[Xk+llk]  “  WklkJ  +  E(wk]  +  (<f>  -  K,) xklk  (2.15) 

Since  the  values  of  the  expectations  are  zero  by  Equations  (2.3)  and  (2.13),  we  get 

Kj  -  <t>.  (2.16) 

So  the  new  form  of  Equation  (2.9)  is 

*k+llk  *  ^kik-  (2.17) 

Proceeding  in  a  similar  fashion,  we  substitute  Equations  (2.1)  and  (2.10)  into 
Equation  (2.12)  to  give 

^k+llk+l  =  ^xk  +  wk  '  ^2xk*llk'  ^Szk+1‘  (2.18) 

Now  use  Equation  (2.2),  regroup  the  terms,  and  take  the  expectation,  using  Equation 
(2.13),  to  get  the  value  for  K2. 

K2  *  I  -  KSH  (2.19) 

Combining  Equations  (2.10),  (2.17),  and  (2.19),  the  form  for  the  Kalman  filter 
equations  can  be  written  as 
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and 


(2.20) 


*k+llk  “  ^klk 

*k+iik+i  “  (J  “  GH)xk+1|k  +  Gzk+1  (2.21) 

where  G  replaces  Ks  as  the  Kalman  gain  matrix.  Equation  (2.21)  can  also  be  written  in 
the  form 


*k+llk+l  *  *k+llk  +  ^zk+l  ■  H*k+llk)-  (2.22) 

Equations  (2.20)  and  (2.22)  are  known  as  the  time*update  and  the  observation- 
update  equations,  respectively.  In  order  to  solve  for  the  value  of  the  Kalman  gain 
matrix,  G,  we  must  specify  the  error  function  to  be  minimized.  A  reasonable  choice 
would  be  a  quadratic  function  of  the  error.  So  we  will  choose  the  value  of  G  which 
satisfies  Equation  (2.23). 

G  :  nun  (Jk+1  *  E{>[  it+iik+i^k+nk+iH  (2.23) 

Solving  the  above  equation  requires  an  analysis  of  the  error  covariance  matrices.  This  is 
handled  in  the  next  section. 

E.  PROJECTION  OF  ERROR  COVARIANCE 


The  error  covariance  matrices  are  defined  by  Equations  (2.24)  and  (2.25).  These 
matrices  give  a  feeling  for  the  expected  magnitude  of  the  estimation  error.  Strictly,  they 
are  the  covariance  matrices  for  the  errors,  which  are  n-dimensional,  zero-mean, 
Gaussian  random  vectors. 


P 


Pk+llk  *  E^k+lIkMt+lIk) 
k+llk+l  s  ^k+ilk+1*  k+llk+ll 


(2.24) 

(2.25) 
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Rewriting  Equation  (2.11)  as 


*k+ilk  “  ^xkik  +  wk  (2.26) 

and  putting  it  into  Equation  (2.24),  we  get 

Pk.uk  ■  ♦  Q-  <2-27> 

Similarly,  writing  Equation  (2.12)  as 

*k+llk+l  ■  H  -  OH)V„k  -  GVl  (2.28) 

and  putting  it  into  Equation  (2.25),  we  get 

pk+iik+i  -  (I  “  GH)Pk+1|k(I  -  GH)T  +  GRGt.  (2.29) 


It  is  pleasing  to  note  that  the  Kalman  filter  is  able  to  carry  along  its  own  error 
analysis  by  means  of  Equations  (2.28)  and  (2.29).  All  that  remains  is  to  find  the  value 
for  the  Kalman  gain  matrix,  G.  But  to  simplify  the  notation,  we  will  first  define  the 
residual,  and  its  covariance. 

F.  RESIDUAL 

The  residua!  is  the  difference  between  the  observation  and  the  expected  value  of 
the  observation.  This  is  given  by 

rk+i  *  zk+i  "  E[zk+1J.  (2.30) 

Since  the  estimate  is  unbiased,  we  see  that 

£{zk+1]  «  E(Hxk+1]  +  E{vk+1]  (2.31a) 

-  Hxk+llk.  (2.31b) 
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Putting  Equation  (2.31b)  into  Equation  (2.30)  and  expanding  zk+i  with  Equation  (2.2), 
we  get  the  final  form  for  the  residual, 

rk+l  “  Hxk+i  "  H*k+iik  +  vk+1  (2.32a) 

“  H^k+nk  +  vk+i*  (2.32b) 

Using  this  definition  of  the  residual,  the  observation-update  equation  can  be 
written  as 


*k+llk+l  *  *k+llk  +  Grk+i  (2.33) 

and  the  covariance  of  the  residual  is  found  to  be 

V(rk+1]  -  E[rk+1rTk+1]  (2.34a) 

-  HPk+1|kHT  +  R.  (2.34b) 

Having  this,  we  are  ready  to  derive  the  value  for  the  Kalman  gain  matrix. 

G.  KALMAN  GAINS 

Returning  to  Equation  (2.23),  we  see  that  it  can  be  written  in  terms  of  the  error 
covariance  as 


G  :  mui  (Jk+1  *  trace(  Pk+llk+1) }.  (2.35) 

This  is  equivalent  to  minimizing  the  length  of  the  estimation  error  vector.  Doing  so 
requires  that  we  take  the  partial  derivative  of  Jk+i  with  respect  to  G  and  set  it  equal  to 
zero.  Solving  the  resulting  equation  will  yield  the  optimal  value  of  G. 

Taking  the  partial  derivative  of  Jk+i  requires  the  use  of  the  following  matrix 
relation,  where  A  and  B  are  matrices  ar.d  B  is  symmetric: 
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g^[trace(  ABA  )]  -  2AB. 


(2.36) 


Putting  Equation  (2.29)  into  Equation  (2.35),  applying  the  above  relation,  and 
setting  the  result  equal  to  zero  gives 

-2(1  -  GH)Pk+llkHT  +  2GR  -  0.  (2.37) 

Solving  for  G, 

Q  -  Pk,llkHT(HPk,llkHT  *  R)-‘  (2.38a) 

■  Pk.llkHTV[r„.1)-1.  (2.38b) 

It  is  important  to  note  at  this  point  that  the  value  of  G  is  a  function  of  time,  and  will 
henceforth  use  the  time  subscript  k. 

Before  compiling  a  coherent  set  of  Kalman  filter  equations,  we  will  take  the  effort 
to  simplify  Equation  (2.29).  By  putting  Equation  (2.38b)  into  Equation  (2.29),  we  can 
get,  after  some  manipulation, 

Pk+l!k+l  “  Pk+llk 

-  Pk.1,kHTV[rk„)-,HPk.1,k.  (2.39) 

By  expanding  out  the  variance  of  the  residual  and  using  the  matrix  inversion  lemma 
[Ref.  4). 

(A  +  BCD)'1  =.  A'1  - 

A-1B(  C*1  +  DA'ty^DA'1,  (2.40) 

we  can,  with  a  modicum  of  persistence,  rewrite  Equation  (2.29)  as 

pk+1lk+!  -  (I  -  m*k+jar  (2.41) 


2 


H.  KALMAN  FILTER  EQUATIONS 

We  now  have  a  set  of  recursive  equations  which  not  only  calculates  the  time- 
varying  optimal  gain  matrix,  but  also  provides  a  detailed  error  analysis  of  the  estimate. 
These  equations  are  given  in  Table  (2.2)  below. 

TABLE  2.2:  KALMAN  FILTER  EQUATIONS 


Estimate  projection: 

*k+llk  “  ^klk 

(2.42) 

Residual: 

rk+l  “  2k+l  '  H*k+llk 

(2.43) 

Error  projection: 

Pkfl!k  *  ^Pklk^T  +  Q 

(2.44) 

.Residual  covariance: 

V(rk+1j  -  HPk,llkHT  +  R 

(2.45) 

Kalman  gain: 

Gk+1  -  pk+i,kHTV[rk+1]*1 

(2.46) 

Estimate  update: 

xk+llk+l  *  xk+llk  +  Gk+lrk+l 

(2.47) 

Error  update: 

Pk+llkfl  *  (1  ‘  Gk+1H)Pk+1|k 

(2.48) 

These  equations  satisfy  our  definition  of  optimal  estimates  (unbiased  estimates 
which  minimize  Jk+i)  for  a  system  described  by  Equations  (2.1)  through  (2.7).  Their 
recursive  nature  makes  them  ideal  for  software  implementation,  and  requires  only  that 
they  be  given  the  following  initial  conditions; 


initial  estimate: 

*010’ 

(2.49) 

and  error  covariance: 

poio- 

(2.50) 

It  is  important  to  remember  that  the  equations  of  Table  (2.2)  are  only  statistically 
optimal  when  the  system  is  linear,  and  perfect  a  priori  knowledge  of  the  noise  exists. 
The  next  chapter  will  consider  the  case  for  a  non-linear  system  with  perfect  a  priori 
noise  statistics. 
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III.  EXTENDED  KALMAN  FILTER 


Chapter  II  showed  that  the  Kalman  filter  yields  a  statistically  optimal  estimate  for 
the  states  of  a  linear,  time-invariant  (LTI)  system  which  is  corrupted  by  additive,  white, 
Gaussian  noise  (AWGN).  Unfortunately,  many  real  systems  of  interest,  such  as  the 
system  studied  in  this  report,  are  not  LTI.  This  chapter  will  consider  the  time-invariant 
system  which  has  a  linear  state-transition  equation,  but  a  non-linear  observation 
equation.  When  the  Kalman  equations  are  applied  in  this  fashion  to  a  non-linear  system, 
they  are  called  the  extended  Kalman  filter  equations. 

To  derive  the  extended  Kalman  filter,  we  first  define  the  state  equations  for  the 
non-linear  system.  Then  we  linearize  the  Kalman  filter  equations  derived  in  Chapter  II 
to  produce  a  reasonable  estimate  of  the  state.  Finally,  we  give  one  method  for 
improving  the  accuracy  of  the  linearization,  and  hence  the  estimate. 

A.  SYSTEM  MODEL 

The  state  space  representation  for  a  system  with  a  non-linear  observation  equation 
is  given  in  Equations  (3.1)  and  (3.2).  We  see  that  the  only  difference  between  this 
system  and  the  LTI  system  of  Equations  (2.1)  and  (2.2)  is  that  the  observation  matrix.  H, 
is  now  a  function  of  the  state. 


Xk+1  -  +  wk 

(3.1) 

zk+i  *  H(xk+I)  +  Vk+1 

(3.2) 

The  noise  processes,  w*  and  v*,  are  AWGN  with  the  same  statistical  parameters  as  given 
in  Equations  (2.3)  through  (2.7). 

Calculating  the  observation  matrix  requires  an  exact  knowledge  of  the  state  of  the 
system,  which  we  do  not  have.  But  we  need  the  value  of  H  in  order  to  calculate  the 
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estimate  of  the  state.  This  circular  dependence  requires  an  approach  which  can  separate 
the  calculations  without  creating  excessive  approximation  errors.  The  approach  we  chose 
is  to  expand  the  value  of  H  in  a  power  series  about  the  current  estimate,  and  then 
maintain  only  the  first  order  terms. 

B.  FIRST-ORDER  APPROXIMATION 

Power-series  expansion  can  be  applied  to  any  function  which  possesses  derivatives 
of  all  orders  in  the  region  of  expansion.  The  expansion  of  the  function  f(x)  about  the 
point  r  is  given  as 

'W  -  E  A  fn(x)l(x  =r)(x  -  r)n  (3.3) 

where  fn(x)|(x=r)  is  the  nth  derivative  of  f  with  respect  to  x,  evaluated  at  x  »  r.  This 
expansion  is  valid  because  the  error  between  the  summation  and  the  actual  value  tends 
to  zero  as  N  tends  to  infinity. 

Jin^tf^x)  -  f(x)J  -  0.  (3.4) 

Performing  this  expansion  on  the  observation  matrix  about  the  time-update  for  the 
estimate  and  keeping  only  the  first  two  terms  gives 

H(xk+1)  =  H(xk+llk)  +  -f-  H(  xk+1|k)[xk+1  -  xk+1)k]  (3.5a) 

k+i 

=  H(xk+1|k)  +  -J-  H(xk+ltk)  Xk+1|k.  (3.5b) 

m  k+l 

x 

With  Equation  (3.5b)  we  are  now  able  to  proceed  with  the  Kalman  filter  analysis  in  a 
fashion  similar  to  Chapter  II. 
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1.  Linearization 


Our  choice  of  point  about  which  to  linearize  H  is  critical  to  the  derivation  of 
the  Kalman  equations.  The  point  we  chose,  the  time-update  of  the  estimate  (also  called 
the  estimate  projection),  is  the  estimate  based  on  everything  up  to,  but  not  including, 
the  current  observation.  This  is  the  best  choice  for  the  linearization  because  it  accounts 
for  all  available  information  just  prior  to  needing  H  for  further  calculations. 

Methods  for  improving  the  accuracy  of  this  linearization  include  keeping 
second-order  terms  in  the  series  expansion  and  iterating  over  the  point  of  expansion. 
The  former,  while  mathematically  sound,  can  prove  difficult  to  implement  due  to  the 
need  to  calculate  the  second  partial  derivative  of  a  matrix.  For  this  reason,  we  chose  to 
implement  an  iterative  approach  which  is  discussed  later  in  this  chapter. 

2.  Observation  Matrix 

Before  we  begin  substituting  the  linearized  observation  matrix  into  the  linear 
Kalman  equations,  we  will  employ  one  more  term  for  notational  simplicity.  By  defining 

H'  =  ^r—  H(  W  (3.6) 

^  k-*-  Ilk 

we  can  write  Equation  (3.5b)  as 

H(W  =  H(xk+1|k)  +  H’Xk+m  (3.7) 

where  the  time  dependency  for  H'  is  understood  to  be  k+l|k.  This  notation  will  allow  us 
to  write  the  extended  Kalman  filter  equations  in  a  form  similar  to  the  ones  derived  in 
Chapter  11.  An  interesting  (and  important)  consequence  of  Equation  (3.7)  can  be  found 
by  taking  the  expectation  of  both  sides  of  it,  as  seen  in  Equation  (3.8).  This  relation 
will  prove  critical  to  the  development  of  the  extended  Kalman  filter  equations. 
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E(H(xk+l))  -  H(xk+1|k) 


(3.8) 


3.  Linear,  Recursive  Form 

For  simplicity  and  efficiency  we  choose  a  linear,  recursive  set  of  equations 
for  our  estimates,  just  as  we  did  in  Chapter  II.  In  this  case,  however,  we  start  with  only 
the  estimate-update  equation,  ■ 


xk+llk+l  “  ak+lik  +  ®k+lzk+l  ^.9) 

and  then  solve  for  ak+i|k  by  requiring  that  the  estimate  errors  be  unbiased.  Defining  the 
estimate  errors  to  be 


^k+llk  *  xk+l  '  xk+llk 

(3.10) 

k+llk+l  *  xk+l  '  xk+llk+l 

(3.11) 

and  substituting  Equations  (3.2),  (3.9),  and  (3.10)  into  Equation  (3.11)  yields 


xk+llk+l  M  Xk+llk  +  xk+llk 

‘  ak- l:k  "  Gk+j(H(Xk+1)  +vk*i)-  (3.12) 

Taking  the  expectation  of  both  sides  of  Equation  (3.12)  gives  the  value  for  ak+i|k  as 

ak+iik  *  *k+iik  "  Gk+1H(  xk+1)k) .  (3.13) 

By  defining  the  residual  as  the  difference  between  the  observation  and  the  expected 
value  of  the  observation. 
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rk+l  “  zk+l  “  ^zk4-a^ 


-  zk+1  -  H(xk+llk)  (114) 

we  can  combine  Equations  (3.9),  (3.13),  and  (3.14)  to  yield  the  standard  extended 
Kalman  filter  estimate-update  equation, 

*k+llk+l  *  *k+l|k  +  Gk+lrk+l-  (H5) 


4.  Projection  of  Error  Covariance 


Having  the  form  of  the  extended  Kalman  equations,  we  can  now  proceed  in 
a  manner  similar  to  that  of  Chapter  II.  Using  Equations  (3.10)  and  (3.11),  and  the 
definitions  of  error  covariance 


and 


Pk+llk  *  Et*k»llk*  k+llkl 


(3.16) 

Pk+llk+l  *  E£*k+llk+l*  k+llk+l)*  (117) 

we  will  derive  the  error-projection  and  error-update  equations.  Putting  Equation  (3.1) 
into  Equation  (3.10)  and  using 


*k+llk  *  ^klk  (118) 

we  can  write  Equation  (3.10)  as 

*k+llk  “  ^Xklk  +  wk-  (119) 

Now  using  Equation  (3.19)  in  (3.16)  we  get 

pk+uk  -  «WT  +  Q-  (3.20) 
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Finding  the  equation  for  Pk+i|k+i  will  require  the  use  of  the  vector  gradient 
of  H,  as  given  in  Equation  (3.6).  Starting  with  Equation  (3.11)  and  substituting  into  it 
Equations  (3.9)  and  (3.13)  gives 

*k+llk+l  ■  xk+l  "  xk+llk 

-  <W>W  -  H(xk<>)).  <3.:i> 

By  using  Equations  (3.2)  and  (3.7),  we  can  put  the  error  into  the  form 

*k+llk+l  "  xk+l  ‘  xk+llk 

-  Gk+1[H(xk+1)  +  vk] 

+  Gk+1^  xk+llk) 

*  xk+l  *  xk+llk 
*  Gk+lvk 

-  Gk+j[H(  xk+j)  -  H(xk+1,k)] 

“  *k+llk  ‘  Gk+lvk 
"  Gk+lH’xk+llk 

-  [I  -  Gk+1H’l*k+llk  -  Gk+1vk.  (3.22) 

This  gives  us  an  equation  for  Pk+i|k+i 

Pk*i,k+i  -  U  -  CH*)Pk+1Ik(I  -  GH’)T  +  GRGt  (3.23) 

which  is  identical  in  form  to  Equation  (2.29),  the  covariance  update  for  the  linear 
Kalman  filter,  with  the  substitution  of  H'  for  H. 

5.  Variance  of  the  Residual 

The  one  final  step  required  prior  to  deriving  the  Kalman  gains  is  to  get  an 
expression  for  the  variance  of  the  residual,  much  as  we  did  in  Chapter  II.  By 
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substituting  Equations  (3.2)  and  (3.7)  into  the  definition  of  the  residual,  Equation  (3.14), 
we  get 

rk+i  *  H<xk+i)  +  vk  *  H(*k+iik) 

-  H’5Ek+llk  +  vk.  (3.24) 

The  covariance  of  Equation  (3.24)  is 

Vlrk„]  -  «wtmI  <3.25a) 

-  H'P,„llkH'T  *  R.  (3.25b) 

6.  Kalman  Gains 

The  similarity  between  the  linear  and  non-linear  error  covariance  equations 
is  both  pleasing  and  fortunate.  Since  Equations  (3.20)  and  (3.23)  are  similar  in  form  to 
Equations  (2.27)  and  (2.29),  we  can  use  our  results  from  Chapter  II  to  derive  the  Kalman 
gains  for  the  non-linear  system.  Specifically,  the  value  of  Gk+i  which  satisfies  the 
following  condition 

G  :  nun  {Jk+J  =  trace(Pki„k+1)},  (3.26) 

is  found  by  taking  the  partial  derivative  of  Jk+i  with  respect  to  Gk+i  and  setting  it  equal 
to  zero.  Solving  the  resulting  equation  yields  the  optimal  value  of  Gk,i, 

♦  R>‘‘ 

-  (3.27) 
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C.  ITERATED  LINEARIZATION 


As  mentioned  earlier  in  this  chapter,  one  method  for  improving  the  accuracy  of 
the  extended  Kalman  filter  is  to  linearize  the  observation  matrix  iteratively  about 
successively  improved  estimates  of  the  current  state.  This  iterative  technique  will  be 
detailed  in  this  section. 

We  will  denote  the  ith  estimate  of  Xk+i  prior  to  the  observation  at  time  k+1  to  be 


xi>+llk 

where  we  initialize  the  iteration  by  setting 

^OJc+lik  *  xk+llk- 

We  can  now  rewrite  Equations  (3.6)  and  (3.7)  as 

Hi’  5  q7T  H(*ijt+llk) 

.  m  k+llk 

and 

Hi(xk+1)  «  HCXjjc+Uk)  +  Hi’*i,k+iik 
where  the  errors  are  defined  as 


and 


xik+llk  *  xk*l  "  ^.kflik 
xi,k+llk+l  =  xk+l  "  ^ijt+lik+l- 


By  combining  Equations  (3.13)  and  (3.30),  and  recalling  that 


E(xk+l]  *  ^txk+likl* 

c 

we  get  the  equation  for  the  iterated  estimates, 


(3.28) 

(3.29) 

(3.30) 

(3.31) 

(3.32) 

(3.33) 

(3.34) 
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A  A 

Xi+ljt+llk  *  ^k+llk+l 

*  xk+llk  +  G;k+ltzk+l  "  H(*i*+llk) 

-  Hj’(xk+ltk  -  Xjj|+1|k)].  (3.35) 

Expressing  this  iterative  procedure  as  an  algorithm  gives  insight  into  its  simplicity 
and  power.  The  algorithm  is  given  in  pseudo-code  in  Table  (3.1). 

TABLE  3.1:  ITERATION  ALGORITHM 
calculate  Pk+1|k 

set  *0*+l|k  =  *k+l|k 

set  i  *  0 
do 

calculate  H;’ 
calculate  Gik+1ik 
calculate  xi>+iiktl 

set  Xi+lk+Hk  m  *ik+llk+l 
set  i  *  i  +  1 

until  ( i  >  im#x  or  ^ 

(xi+l^+llk  “  <  £) 

calculate  Pk+1,k+1 

The  criteria  for  stopping  the  iteration  will  usually  be  when  either  i  (the  iteration  index) 
has  reached  a  preset  limit  or  the  difference  between  successive  iterated  estimates  is 
below  a  specified  tolerance.  With  this  algorithm,  and  the  equations  previously  derived, 
we  will  present  the  extended  Kalman  filter  equations  after  a  brief  observation  about 
optimality. 


D.  COMMENTS  ON  OPTIMALITY 


We  derived  the  extended  Kalman  filter  equations  by  using  the  linear  Kalman 
equations  of  Chapter  II  as  a  guide.  While  we  showed  that  the  linear  equations  were 
statistically  optimal,  we  make  no  similar  claims  for  the  non-linear  case.  We  cannot,  in 
fact,  claim  that  the  equations  in  Table  (3.2)  will  converge  to  a  reasonable  solution.  What 
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we  do  state  is  that  the  approach  used  to  derive  these  equations  was  based  on  the  same 
principles  and  procedures  as  was  used  for  the  linear  Kalman  equations.  Based  upon  this 
and  previous  experience,  it  is  reasonable  to  assume  that  the  equations  given  in  Table 
(3.2)  will  yield  a  good,  though  sub-optimal,  estimate  of  the  state. 

E.  EXTENDED  KALMAN  FILTER  EQUATIONS 

The  complete  set  of  iterated,  first-order,  extended  Kalman  filter  equations  are 
given  in  Table  (3.2).  The  iteration  index,  i,  goes  from  zero  to  some  preset  limit.  This 
limit  is  set  only  to  limit  computational  expense. 

TABLE  3.2:  ITERATED,  FIRST-ORDER,  EXTENDED  KALMAN  EQUATIONS 


Error  projection: 

Pk+llk  *  ^Pklk^T  +  Q 

(3.36) 

Estimate  projection: 

xk+lik  *  ^xklk 

(3.37) 

Iteration  seed: 

*OJc+l,k  *  xk+llk 

(3.3S) 

Begin  Iteration 

Observation  matrix: 

Hi’  -  j-r—W 
^  k+llk 

(3.39) 

Residual: 

ri,k+l  *  zk+l  “  ^  Xik+Uk^ 

(3.40) 

Residual  covariance: 

-  H,’PMkH,’T  *  R 

(3.41) 

Kalman  gain: 

(3.42) 

Estimate  update: 

xik+llk+l  =  xik+llk  +  'Jik+lrk+l 

-  Hj’(xk+1|k  -xlk+1,k) 

(3.43) 

End  Iteration 

Error  update: 

Pk+l|k+l  *  ^  "  ^k+l^’)Pk+llk 

(3.44) 
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IV.  MODEL  OF  THE  TRACKING  SYSTEM 


In  this  chapter  we  introduce  the  mathematical  model  of  the  tracking  system  which 
we  will  use  for  the  Monte  Carlo  simulations  throughout  the  remainder  of  this  report. 
We  start  by  presenting  the  physical  relationship  between  the  target  and  the  observer.  We 
then  show  how  this  can  be  represented  in  the  state  space  for  use  with  the  extended 
Kalman  filter  equations  derived  in  Chapter  IV.  Following  that,  the  noise  processes 
involved  are  explained  and  the  appropriate  equations  derived.  Having  a  state  space 
description  of  the  noisy  tracking  system,  we  then  define  the  error  function  used  as  a  test 
statistic  for  performance  of  the  Kalman  filter.  Finally,  we  explain  the  five  scenarios 
which  were  used  for  the  Monte  Carlo  simulations. 

A.  PHYSICAL  SYSTEM 

The  system  used  in  this  report  consists  of  a  single  observer  and  a  single  target. 
The  coordinate  system  is  a  two-dimensional  Cartesian  coordinate  system  with  the 
observer  at  the  origin  and  the  target  initially  located  in  the  first  (upper  right)  quadrant 
as  shown  in  Figure  (4.1).  The  x  and  y  axes  correspond  to  East  and  true  North, 
respectively.  The  target  is  free  to  move  unrestricted  throughout  the  coordinate  space, 
while  the  observer  is  stationary  and  remains  at  the  origin. 

The  observation  information  received  by  the  observer  consists  of  bearing  and 
range  measurements  from  the  observer  to  the  target.  This  is  consistent  with  the 
measurements  supplied  by  most  types  of  radar  in  use  today.  These  measurements  are 
taken  at  equal  time  intervals  of  10  seconds  (T  =  0.002778  hours).  This  simple  system 
will  now  be  transformed  into  state  space  notation. 
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Figure  4.1:  Physical  Layout  of  Target  and  Observer 


B.  STATE  SPACE  MODEL 

The  discrete-time,  state  space  model  of  the  system  depicted  by  Figure  (4.1)  is 

Xk+1  =  0Xk  +  wk  (4.1) 

where  the  state  vector,  X,  is  defined  to  contain  the  minimum  number  of  elements 
necessary  to  describe  the  target  in  terms  of  its  degrees  of  freedom.  As  such,  X  consists 
of  the  position  and  velocity  of  the  target.  (The  capital  letter,  X,  is  used  in  this  chapter 
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to  denote  the  state  to  preclude  confusion  with  the  coordinate,  x.  The  rest  of  this  report 
conforms  with  the  standard  practice  of  using  a  lower  case  x  for  the  state,  as  coordinates 
are  not  referenced  outside  of  this  chapter  and  the  appendices.)  The  unknown  (i.e., 
unpredictable)  accelerations  of  the  target  are  accounted  for  by  the  state  excitation 
vector,  wit. 


X  ■ 


(4.2) 


The  matrix  <f>  in  Equation  (4.1)  is  chosen  to  fit  the  target  dynamics.  The  two  types 
of  target  dynamics  which  are  normally  assumed  are  stationary,  and  moving  lineariy  at 
constant  velocity.  The  appropriate  4>'s  for  these  two  cases  are  given  in  Equations  (4.3) 
and  (4.4). 


Stationary. 


*x 


- i 0  0  0  ” 
0  10  0 
0  0  10 
_  0  0  0  1  _ 


(4.3) 


Moving  Linearly 


”  1  T  0  0  " 
0  10  0 
0  0  1  T 
_  0  0  0  1  _ 


(4.4) 


The  constant,  T,  is  the  observation  interval  mentioned  previously.  Since  we  design  for 
the  most  general  case,  we  use  fa  for  all  of  the  simulations. 
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Putting  Equations  (4.2)  and  (4.4)  into  Equation  (4.1)  gives  the  final  state  equation 
for  the  simulated  system.  We  will  now  present  the  observation  matrix  and  derive  the 
first-order  linearization  required  by  Chapter  III. 

C.  OBSERVATION  MATRIX 

The  observation  equation  for  our  system  consists  of  noise  added  to  bearing  and 
range  measurements.  Since  the  bearing  and  range  to  the  target  are  non-linear  functions 
of  x  and  y,  the  observation  equation  is  given  as 


zk+i  »  H(Xk+1)  +  vk+1  (4.5) 

where 

■■[;].  <4-« 

The  bearing  and  range  are  defined  by 

bearing:  P  =  tan'^yj  (4.7) 

and  range:  P  *  «j  x2  +  y2  .  (4.8) 

The  extended  Kalman  filter  equations  derived  in  Chapter  III  require  the  vector 
gradient  of  the  observation  matrix  in  the  state  space.  This  matrix  is  given  as 

H’  =  —jH(X)  (4.9) 

dX 

d  a  d  a  d  a  d  a 

s*  »r/  if  wf 

d  d  d  d  . 
dKP  BvJ1  d\’yP 

where  the  partial  derivatives  are  evaluated  at  the  estimate. 
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By  putting  Equations  (4.7)  and  (4.8)  into  Equation  (4.10)  and  doing  ths  partial 


differentiations  we  get  the  expressions  for  the  elements  of  the  H’  matrix. 


(4.11a) 

4"-° 

(4.11b) 

&  ■  ° 

X 

(4.11c) 

5TP  "  0 
y 

(4.  lid) 

(4.12a) 


(4.12b) 


1  i 

2  P 

y 

p 


2y 


(4.12c) 


( 4. 12d) 


Using  Equations  (4.11)  and  (4.12)  in  Equation  (4.10)  gives 


(4.13) 
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which  is  required  for  the  extended  Kalman  filter  analysis.  We  will  now  discuss  the  noise 
processes  inherent  in  the  system. 

D.  NOISE 

The  noise  processes  which  are  denoted  by  w*  and  v*  in  Equations  (4.1)  and  (4.5) 
are  assumed  to  be  additive,  white,  Gaussian  noise  (AWGN)  with  the  auto-  and  cross- 
correlation  properties  described  by  Equations  (2.3)  through  (2.7).  The  covariance  matrix 
for  the  observation  noise  is 


R  - 


(4.14) 


where  the  values  used  for  the  variations  are 

ojj  -  0.0005  rad2  (4.15) 

and 

crp2  *  0.0005  km2.  (4.16) 

We  felt  that  these  values  were  representative  of  actual  radar  accuracies  and  provided 
nominal  values  about  which  to  perform  the  sensitivity  analysis  detailed  in  Chapter  V. 

The  analysis  of  the  state  excitation  covariance  matrix,  Q,  is  considerably  more 
complex  and  is  derived  in  Appendix  A.  The  purpose  of  Q  is  to  account  for  the 
unknown  accelerations  of  the  target.  The  results  of  the  derivation  are  that  Q  is  given  as 


f  30  3«*y]  1  T 
q  *  r  rT 

Le^]  na/ij 

where  the  matrix  elements  are 


(4.17) 

‘l 
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Wl  "  [\]  a*  +  vvV 

(4.18) 

E[ay2]  »  ffv2  +  vx2ce2 

(4.19) 

and 

E(axay]  -  E(ayax]  - 

vxvy_(%]  -ffe]  (4-2°) 


and  the  state  noise  coefficient  matrix  is 


where  T  =  0.002778  hours. 

The  purpose  of  Equations  (4.17)  through  (4.20)  is  to  take  the  values  of  the 
variances  of  linear  and  tangential  acceleration  and  transform  them  into  the  Cartesian 
coordinate  system  of  the  state  space.  The  values  for  these  variances  were  assumed  to  be 


(linear)  oy2  =  0.001  (km/hr2)2  (4.22) 

(tangential)  oQ2  =  0.001  (rad/hr2)2  (4.23) 

which  are  characteristic  of  a  non-maneuvering  target.  These  values  were  intentionally 
chosen  small  so  as  to  make  the  Kalman  filter  believe  that  the  target  was  non¬ 
maneuvering.  The  effects  of  this  choice  will  be  shown  in  Chapter  VII.  Having  a  model 
for  the  noise  processes  in  the  system,  we  will  now  define  a  performance  measure  for  the 
estimates. 
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E.  DEFINITION  OF  ERROR  FUNCTION 

The  choice  of  error  (or  performance)  function  is  an  arbitrary  one.  Its  only 
purpose  is  to  provide  us  with  some  statistic  to  use  as  a  measure  of  filter  performance. 
This  statistic  is  then  the  value  which  determines  the  relative  quality  of  the  various  filter 
estimates.  For  this  report  we  chose  a  weighted  sum  of  the  filter  errors  as  the  error 
function. 

1/ 

max 

X,  kT|xk  -  *klk|  (4.24) 

k=l 

By  taking  a  weighted  sum  of  the  estimate  errors  we  can  weight  the  final  estimates 
more  heavily  than  the  early  estimates.  The  weighting  factor,  kT,  is  equal  to  the  absolute 
time  since  the  initial  observation.  We  feel  that,  although  this  is  not  the  only  possible 
error  function,  it  is  certainly  a  reasonable  one.  With  this  error  function  we  can  now 
measure  the  performance  of  the  Kalman  filter  in  the  various  scenarios  described  in  the 
next  section. 

F.  SIMULATION  SCENARIOS 

For  this  report  we  chose  to  run  the  adaptive  Kalman  filter  on  five  different 
scenarios.  Each  scenario  is  described  by  the  positions,  velocities  and  accelerations  of  the 
target  and  obsener.  The  observer  was  chosen  to  be  stationary  at  the  origin  of  the 
Cartesian  coordinate  system  as  shown  previously  in  Figure  (4.1).  The  target  dynamics 
are  given  in  Table  (4.1)  and  Figures  (4.2)  through  (4.5)  for  each  scenario.  The  tangential 
accelerations  are  given  in  g  units  where  a  g  is  defined  as 

g  =  9.8  m/sec2  (4.25) 

and  the  observation  inters a»,  T,  is  0.002778  hours  (10  seconds). 
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TABLE  4.1:  TARGET  DYNAMICS  BY  SCENARIO 


Scenario  # 

xo 

y0 

VxO 

vyO 

acctw»|«nti»l 

duration 

(km) 

(km) 

(km/ hr) 

(km/ hr) 

(g’s) 

(sec) 

1 

5 

37 

0 

0 

0 

0 

2 

5 

37 

430 

0 

0 

0 

3 

5 

37 

400 

•280 

0 

0 

4 

5 

37 

600 

50 

0.8 

40 

5 

5 

37 

450 

-300 

-0.8 

50 
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Figure  4.2:  Scenario  #2 
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Scenario  #3 
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34 


50 


Scenario  #5 


We  are  now  ready  to  delve  into  the  details  of  the  adaptive  methods  which  we 
have  proposed.  The  noise  adaptation  is  covered  in  Chapter  V  and  the  adaptive 
maneuver  gating  is  treated  in  Chapter  VI.  Following  that,  the  simulations  are  presented 
in  Chapter  VII  and  the  results  in  Chapter  VIII. 
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V.  NOISE  ADAPTATION 


This  chapter  begins  by  explaining  the  critical  dependence  of  the  Kalman  filter’s 
performance  on  the  quality  of  the  a  priori  noise  estimates.  We  will  both  argue  and 
demonstrate  that  the  Kalman  filter  achieves  its  optimal  performance  when  the  a  priori 
*  measurement  noise  vk  is  equal  to  the  actual  measurement  noise.  Having  shown  this,  we 
then  present  two  methods  for  estimating  the  actual  observation  noise  of  the  system. 
Finally,  we  use  this  estimate  of  the  actual  observation  noise  to  adapt  the  Kalman  filter 
to  improve  its  performance. 

A.  NOISE  SENSITIVITY 

A  rigorous  derivation  of  the  effects  of  incorrect  a  priori  noise  estimates  on  the 
performance  of  the  Kalman  filter  is  well  beyond  the  scope  of  this  report.  We  have  not, 
in  fact,  been  able  to  find  such  a  derivation  anywhere  in  the  literature.  As  such,  we  will 
present  our  case  based  upon  reasonability  arguments  and  support  our  statements  with 
sample  simulations. 

In  order  to  proceed  with  this  discussion,  it  is  necessary  to  make  a  clear  distinction 
between  the  Kalman  parameters  which  are  calculated  with  incorrect  (assumed)  a  priori 
information  and  those  which  are  calculated  with  conect  u  priori  information.  For  our 
purposes  we  shall  use  the  following  notation. 


assumed: 

Pk+llk+1  =  (I 

*  ^k+l^)Pk+llk 

(5.1) 

correct: 

P°k+l,k+l  -  (I 

-  G°k+1H)P°k+1|k 

(5.2) 

Our  goal  then,  is  not  to  minimize  some  function  of  Equation  (5.1),  but  to  minimize  some 
function  of  the  difference  between  Equations  (5.1)  and  (5.2).  We  will  use  the  simplest 
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of  minimization  functions,  and  just  minimize  the  difference  for  any  arbitrary  value  of 
Pk+i|k  bv  setting 


Pk+llk  *  p°k+llk  “  0*  (5.3) 

Putting  Equations  (5.1)  and  (5.2)  into  Equation  (5.3)  we  find  that 

^k+llk+i  ■  P  k+llk+1  *  (G°k+1  -  Gfc+i)  HPk+1)k 

-  0.  (5.4) 

The  only  way  to  solve  Equation  (5.4)  for  arbitrary  Pk+i|k  is  by  setting 

Gk+1  =  G°k+i.  (5-5) 

Equation  (5.5)  gives  the  expected  (and  pleasing)  result  that  the  optimal  estimates 
are  achieved  only  if  the  Kalman  gains  are  equal  to  the  gains  calculated  from  correct  a 
priori  information.  Though  this  result  is  not  unexpected,  it  is  often  not  appreciated  in 
physical  implementations  of  Kalman  filters. 

The  effect  of  incorrect  a  priori  noise  information  is  shown  graphically  in  Figure 
(5.1).  This  graph  was  produced  by  setting  the  a  priori  noise  covariance  equal  to  the 
actual  noise  covariance  scaled  by  the  factor  Kr  (noise  covariance  scale)  and  running  the 
simulations  described  by  scenario  three  of  Table  (4.1). 

priori  *  (5.6) 

Although  the  exact  shape  of  Figure  (5.1)  is  dependent  upon  the  nature  of  the  system 
being  simulated,  it  is  characteristic  of  the  results  we  expect.  Figure  (5.1)  shows  clearly 
that  the  best  performance  (lowest  total  error)  was  achieved  when  the  a  priori  R  was 
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equal  to  the  actual  value  of  R  (Kr  ■  1).  This  result  can  best  be  explained  by 
consideration  of  the  information  carried  by  the  observations. 


One  way  of  looking  at  the  operation  of  the  Kalman  filter  is  to  consider  the 
observations  as  being  a  combination  of  information  and  noise.  The  Kalman  filter  is  then 
a  device  which  extracts  the  information  from  the  observations  and  rejects  the  noise. 
The  Kalman  gains  are  then  seen  to  be  directly  related  to  the  information  in  the 
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observations  and  inversely  related  to  the  noise.  As  the  information  goes  up,  the  gains  go 
up;  as  the  noise  goes  up,  the  gains  go  down.  The  goal  of  the  Kalman  equations  is  to  set 
the  gains  to  maximize  the  information  taken  from  the  observations  while  minimizing  the 
effects  of  the  noise.  This  is,  of  course,  predicated  on  the  a  priori  values  for  the  noise 
that  we  give  to  the  Kalman  filter. 

If  the  a  priori  value  of  R  is  larger  than  the  actual  (Kr  >  1)  then  the  Kalman  gains 
will  be  lower  than  optima!  and  the  filter  will  reject  some  of  the  information  in  the 
observations  along  with  the  noise.  If  the  a  priori  value  is  too  small,  then  the  gains  will 
be  too  large  and  the  filter  will  treat  some  of  the  noise  as  information.  The  Kalman  is 
thus  dependent  upon  our  choice  of  a  priori  values  of  R  and  requires  them  to  be  accurate 
for  its  estimates  to  be  optimal. 

To  satisfy  this  requirement  we  must  give  the  Kalman  filter  either  perfect  a  priori 
noise  information,  or  else  the  ability  to  estimate  and  adapt  to  the  actual  value  of  R. 
Since  perfect  a  priori  information  is  only  a  concept  (as  opposed  to  reality),  we  have 
chosen  to  implement  a  method  which  will  allow  the  Kalman  filter  to  adapt  to  the  actual 
values  of  the  noise. 

B.  ESTIMATING  OBSERVATION  NOISE 

Before  the  Kalman  filter  can  adapt  to  the  actual  observation  noise  covariance  it 

must  be  able  to  form  an  estimate  of  the  matrix  R.  Since  the  only  information  available 

to  the  Kalman  filter  comes  from  the  observations,  it  must  calculate  an  estimate  of  R  as  a 

function  of  the  observations.  We  will  define  the  estimate  of  R  for  observations  from 

l 

time  k  =  n  through  k  =  m  as 


R 


n/n 


ft  zn,zn+l’'"’zm^ - 


(5.7) 
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Since  the  observations  for  our  system  are  given  as 


zk+1  *  H(  xk+1)  +  vk+1  (5.8) 

we  cannot  directly  separate  the  noise  from  the  observation  without  an  exact  knowledge 
of  the  state,  x^iitc-  But  if  we  had  this  knowledge,  there  would  be  no  need  to  use  a 
Kalman  filter.  The  best  we  can  do  is  to  use  the  residual,  rk+i,  which  provides  an 
estimate  of  the  observation  noise. 

rk+l  “  zk+l  "  H(*k+l|k)  (5.9) 

This  makes  our  estimate  of  R  in  Equation  (5.7)  a  function  of  both  the  observations  and 
the  state  estimates. 


^n/n  *  ^  zn’  •  (5.10) 

Since  the  state  estimates  are  functions  of  the  observations  it  might  seem  unnecessary  to 
use  them  in  Equation  (5.10).  We  do  this,  however,  because  the  state  estimates  are  also 
functions  of  other  parameters,  notably  the  a  priori  value  of  R.  So  we  expect  different  a 
priori  values  of  R  to  produce  different  state  estimates  for  the  same  set  of  observations. 
This  is  exactly  what  Figure  (5.1)  demonstrates.  We  will  now  present  two  different 
functions  for  Equation  (5.10)  which  can  produce  estimates  of  R. 

1.  Recursive  Variance 

The  most  direct  (and  least  memory  intensive)  method  of  calculating  an 
estimate  of  the  noise  variance  is  to  assume  that  R  is  a  diagonal  matrix  (no  cross¬ 
correlation  terms)  and  to  recursively  calculate  the  variance  of  each  component.  This 
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only  works  if  we  assume  that  the  residual  and  the  observation  noise  are  both  zero-mean. 
The  function  which  calculates  the  recursive  variance  is  given  as 


Ww  ■  CT'Wk  *  ElWW2  k  *  ■•••»  <51l> 

where  the  index  i  spans  the  number  of  elements  in  the  residual  vector.  We  then  apply 
Equation  (5.11)  to  each  element  .of  the  residual  vector  independently. 

This  method  has  the  advantages  of  simplicity  and  ease  of  implementation. 
Its  two  disadvantages  are  that  it  requires  many  (30  to  40)  observations  before  its 
estimates  become  meaningful  and  that  it  ignores  any  possible  cross-covariances  of  the 
noise  terms.  The  former  is  rooted  in  stochastic  theory  and  cannot  be  circumvented;  the 
latter  is  solved  by  the  following  method. 

2.  Time  Series  Covariance 


Although  we  have  assumed  that  the  noise  components  were  uncorrelated  to 
simplify  the  derivations,  we  will  now  attempt  to  estimate  the  possibly  non-zero  off- 
diagonal  terms  of  the  matrix  R.  In  order  to  do  this  we  will  use  a  batch  processing 
function  which  uses  all  of  the  residuals  at  nee,  instead  of  one  at  a  time,  as  the 
recursive  method  does.  This  function  requires  a  time  series  of  residuals  as  its  argument, 


*  m-n  ( zk  ^n^r»^zk  “  ^nfn) 


m 


(5.12) 


where  the  mean  of  the  residuals  is  given  by 


»  «  1  y  z 

njn  m-n+1  k 


(5.13) 
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Equations  (5.12)  and  (5.13)  are  defined  as  the  sample  variance  and  the  sample  mean  of 
the  parameter  Zk. 

It  is  interesting  to  note  that  the  Kalman  filter  equations  already  provide  an 
expected  value  for  the  observation  covariance  matrix  which  is  estimated  by  Equations 
(5.12)  and  (5.13).  This  value  is  calculated  with  each  new  observation  and  is  the  expected 
covariance  of  the  residual, 

Vlvd  -  MWH-t  ♦  R„  priori.  (5.14) 

Equating  this  to  our  estimates  we  find  that 

-  H'Pm»wH'T  *  R„  prior/.  (5.15) 

Solving  Equation  (5.15)  for  the  value  of  R»  priori  gives  us 

Ra  priori  •  -  H’Pmlm.jH'T  (5.16) 

which  could  be  used  as  an  estimate  for  the  actual  value  of  R.  The  problem  with 
Equation  (5.16),  though,  is  that  the  difference  of  two  positive  semi-definite  matrices  is 
not  necessarily  another  positive  semi-definite  matrix.  Sir  .•  we  require  R  to  be  positive 
semi-definite  (noise  cannot  have  negative  power),  the  use  of  Equation  (5.16)  can,  and 
has.  produced  calculational  singularities  in  the  Kalman  equations. 

Although  this  singularity  problem  C3n  be  contiolled  by  additional  processing, 
we  have  chosen  to  implement  Equation  (5.12)  as  our  estimate  of  the  actual  noise 
covariance.  It  is  more  computationally  stable  than  Equation  (5.16),  more  robust  than 
(5.11),  and  provides  an  estimate  which  is  conservative;  the  expected  value  of  the  estimate 
is  larger  than  the  actual  values,  as  shown  by  Equation  (5.16).  Now  that  we  have  chosen 
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a  method  for  estimating  the  actual  observation  noise  covariance  matrix,  R,  we  will 
present  a  method  for  incorporating  this  information  into  the  Kalman  filter. 

C.  NOISE  ADAPTATION 

The  purpose  of  estimating  the  observation  noise  covariance  is  to  provide  the 
Kalman  filter  with  the  ability  to  both  correct  for  poor  a  priori  estimates  and  to  adapt  to 
non-stationary  noise  processes.  Since  the  primary  objective  of  Kalman  filtering  is  to 
provide  the  best  state  estimates  possible,  we  will  not  allow  our  design  to  be  driven  by 
dogmatic  concerns  for  computational  efficiency.  It  is  expected  that  an  adaptive  process 
will  require  more  processing,  and  we  will  suffer  this,  within  limits.  We  will  also  consent 
to  the  use  of  more  memory  for  the  computations.  Because  these  issues  of  speed  of 
memory  are  being  overtaken  by  technology,  we  will  stretch  our  design  to  maximize  the 
benefits  at  a  moderate  expense  of  processing  and  memory. 

With  this  license  to  moderately  increase  processing  and  memory  requirements,  we 
have  chosen  to  implement  the  observation  noise  adaptation  in  the  form  of  a  smoothing 
process.  This  means  that  the  Kalman  filter  carries  along  a  finite,  growing  memory 
which  stores  the  observations  and  the  residuals.  At  specified  intervals  the  estimate  of  R 
is  calculated  from  the  collected  residuals.  This  new  estimate  of  R  is  used  to  recompute 
the  estimates  from  the  stored  observations.  The  filter  then  releases  the  stored  residuals 
and  observations  and  the  process  repeats. 

This  explanation  is  best  served  by  an  example.  Table  (5.1)  details  the  operation  of 
a  Kalman  filter  which  adapts  every  30  observations.  The  number  of  observations 
between  adaptations  should  be  chosen  on  the  basis  of  a  Monte  Carlo  analysis  for  each 
specific  problem.  Although  this  number  will  vary,  we  have  achieved  consistently  good 
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results  with  a  value  of  30.  This  is  a  reasonable  value  as  it  does  not  require  much 
increase  in  either  processing  speed  or  memory. 

TABLE  5.1:  EXAMPLE  ADAPTATION  ALGORITHM 


Time  index 

Action 

Store 

1 

Compute  x1|0 

*i,  ri 

2 

Compute  x2(1 

Z2»  r2 

30 

Compute  x30129 

Z30»  r30 

30 

Compute  R1)S0 

30 

Recompute  x1)0  through  x30129 

Release  memory 

31 

Compute  x31|30 

z31’  r31 

32 

Compute  x32|31 

z32’  r32 

60 

Compute  x60|59 

z60>  r60 

60 

Compute  R3160 

60 

Recompute  x31130  through  x60|59 

Release  memory 

The  iterative  algorithm  shown  in  Table  (5.1)  can  be  implemented  as  long  as  the 
calculations  needed  to  recompute  the  state  estimates  can  be  completed  between 
observations.  For  our  simulations  this  is  not  a  problem  as  the  observations  occur  only  a 
few  times  per  minute. 

Armed  with  this  method  of  noise  adaptation,  we  will  now  investigate  and  derive  a 
method  for  allowing  the  Kalman  filter  to  adapt  to  a  maneuvering  target.  Following  that, 
we  will  present  the  simulation  results  which  show  the  benefits  of  this  adaptation  scheme. 


44 


VI.  MANEUVER  GATING 


This  chapter  introduces  the  concept  of  correlated  maneuver  gating.  It  begins  by 
defining  the  Mahalanobis  Distance  and  explains  how  it  can  be  used  as  a  test  statistic  for 
maneuver  gating  off  of  the  residual.  We  then  explain  how  the  Kalman  filter  responds  to 
a  detected  maneuver  by  adapting  its  error  covariance  matrix.  This  adaptation  is  usually 
in  the  form  of  a  reset  to  an  initial  value,  but  we  present  a  more  sophisticated  method 
which  we  call  incremental,  correlated  maneuver  gating. 

A.  RESIDUAL  GATING 

The  first  step  in  correcting  for  improper  behavior  in  a  Kalman  filter  is  detecting 
that  the  behavior  has  occurred.  This  is  the  goal  of  maneuver  gating.  Once  we  have 
achieved  a  method  which  has  both  a  high  probability  of  detection  (detecting  maneuvers) 
and  a  low  probability  of  false  alarm  (rejecting  noise),  then  we  will  employ  a  method  to 
correct  for  the  filter’s  behavior. 

The  method  of  adapting  a  Kalman  filter  by  maneuver  gating  has  been  used  widely 
in  the  literature  in  the  past  several  years.  In  every  case  the  test  statistic  used  to 
determine  the  gate  was  one  or  more  elements  of  the  residual,  which  was  compared  to 
some  number  (the  gate).  Often  the  gate  size  was  determined  by  analysis  of  the 
covariance  of  the  residual,  the  very  method  we  shall  use.  What  we  propose,  however,  is 
that  the  entire  residual  be  compared  to  the  covariance  of  the  residual  by  calculating  the 
Mahalanobis  Distance  of  the  residual. 

The  Mahalanobis  Distance,  MD,  of  a  random  number  is  its  ^distance  from  the 
sample  mean  in  units  of  the  sample  covariance.  This  is  given  by 

MD  -  (X  -  M)t  K’1  (X  -  M)  (6.1) 
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where  X  is  the  random  vector,  M  is  the  mean  of  the  stochastic  process,  and  K  is  its 
covariance.  Since  we  are  calculating  this  statistic  for  the  residual,  we  make  the 
assumption  that  the  mean  of  the  residual  is  zero.  If  we  know  that  the  observations  have 
a  predetermined  bias,  we  would  compensate  for  it  so  that  the  residual  would  still  be 
-  zero-mean. 

Since  the  noise  processes  involved  in  our  system  lead  to  residuals  with  a  zero-mean 
Gaussian  distribution  (as  shown  in  Appendix  A),  we  see  that  each  value  of  the 
Mahalanobis  Distance  for  the  residual  describes  a  two-dimensional  ellipse  around  the 
state  estimate  projection.  The  size  of  the  ellipse  is  directly  proportional  to  the 
Mahalanobis  Distance.  This  is  seen  graphically  in  Figure  (6.1).  By  recalling  that  the 
covariance  of  the  residual  is  given  by 


V(r„.J  -  H'lVllkH'T  ♦  R  (6.2) 

we  see  that  the  covariance  of  the  residual  is  a  strong  function  of  the  observation  noise 
covariance.  In  fact,  as  the  error  covariance  decreases,  the  first  term  in  Equation  (6.2) 
becomes  negligible  and  the  observation  noise  covariance,  R,  dominates.  So  the  process 
of  maneuver  gating  reduces  to  the  problem  of  discriminating  between  noise  in  the 
observations  and  a  target  maneuver.  This  will  be  covered  more  thoroughly  in  the  last 
section  of  this  chapter. 

The  determination  of  the  optimal  value  for  the  Mahalanobis  Distance  for  the  gate 
should  balance  the  need  for  early  maneuver  detection  against  the  probability  for  false 
detections.  This  value  needs  to  be  determined  empirically  for  each  implementation  of 
the  Kalman  filter  and  will  be  shown  in  the  following  chapter  to  be  dependent  upon  the 
target’s  actions. 
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The  set  of  observations  and  estimates  in  Figure  (6.1)  demonstrates  the  action  of  the 
Kalman  filter  when  no  maneuver  gating  is  used.  As  seen,  the  estimates  diverge  from  the 
observations  because  the  filter  gains  have  decreased  below  the  value  needed  to  follow  the 
observations.  This  is  a  direct  result  of  the  Kalman  filter’s  error  covariance  matrix 
decreasing  in  magnitude.  In  order  to  allow  the  Kalman  fitter  to  adapt  to  a  maneuvering 
target  we  will  dynamically  adjust  the  magnitude  of  its  error  covariance  matrix.  In  the 
next  section  we  describe  the  widely  used  method  of  error  covariance  reset  as  well  as 
propose  a  method  which  we  call  incremental,  correlated  maneuver  gating. 


(No  Gating) 
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B.  ERROR  COVARIANCE  ADJUSTMENT 


In  Chapters  II  and  III  we  showed  that  the  Kalman  filter  gains  are  a  function  of  the 
state  error  covariance.  As  the  estimated  error  goes  up,  so  do  the  gains.  This  is  the 
filter’s  way  of  compensating  for  a  large  expected  error;  it  weights  the  observations  more 
heavily.  We  can  exploit  this  behavior  of  the  Kalman  filter  and  use  it  to  adapt  the 
Kalman  filter  to  detected  target  maneuvers.  Whenever  we  detect  a  target  maneuver,  '  * 
increase  the  magnitude  of  the  error  covariance  matrix.  This  forces  the  Kalman  filter  to 
move  its  state  estimate  update  closer  to  the  observation.  The  question  remains,  how 
much  do  we  adjust  the  error  covariance  and  how  close  to  the  observation  do  we  want 
the  estimate  to  be? 

1.  Covariance  Reset 

The  commonly  used  method  of  error  covariance  adjustment  is  to  reset  the 
error  covariance  to  its  initial  value  (Po|o).  This  initial  value  is  simply  the  value  set  by 
the  filter  designer  which  allows  the  filter  to  lock-on  to  the  first  observation.  Since  this 
value  is  typically  large  compared  to  the  steady-state  value,  the  Kalman  filter  will  behave 
as  if  it  has  been  reinitialized  whenever  a  maneuver  is  detected. 

This  type  of  covariance  adjustment  will  force  the  Kalman  filter  to  disregard 
all  past  information  and  reinitialize  itself  on  the  current  observation.  This  behavior  will 
be  destructive  when  the  gating  is  a  result  of  anomalous  observation  noise  rather  than 
target  maneuvering.  The  Kalman’s  state  estimate  will  simply  bounce  around  chasing  the 
noisy  observations  and  will  result  in  a  larger  state  estimate  error.  In  addition,  the  large 
magnitude  of  the  initial  error  covariance  matrix  will  require  several  time  intervals  to 
again  reach  a  steady-state  value.  This  method  of  gating  can  therefore  lead  to  an  effect 
which  will  require  several  observations  from  which  to  recover.  What  is  desired  is  a 
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method  of  gating  which  will  allow  the  Kalman  filter  to  "relax"  back  to  a  steady-state 
condition  as  soon  as  possible  after  gating  ceases.  Such  a  method  is  presented  below. 

2.  Covariance  Increment 

Some  of  the  objectionable,  and  persistent,  effects  of  covariance  reset  gating 
can  be  negated  by  increasing  the  error  covariance  matrix  to  a  magnitude  smaller  than  its 
initial  value.  The  determination  of  this  optimal  magnitude,  however,  is  not  immediately 
computable  from  known  data  because  it  is  an  unknown  function  of  the  state  of  the 
Kalman  filter.  Its  value  will  therefore  have  to  be  determined  empirically  from 
experimentation. 

Our  method  for  choosing  the  value  of  the  error  covariance  matrix  consists  of 
incrementally  increasing  the  magnitude  of  the  old  covariance  matrix  until  the  maneuver 
gate  is  satisfied.  An  example  of  this  is  shown  in  Figure  (6.2).  Although  this  method  is 
heuristic  in  nature,  we  have  achieved  consistently  good  results  with  it,  as  the  simulations 
in  Chapter  VII  will  show. 

C.  CORRELATED  GATING 

The  plight  of  reset  gating,  as  described  above,  is  its  vulnerability  to  aberrant  noise 
conditions.  To  counter  this  weakness,  the  maneuver  detection  algorithm  must  make  use 
of  the  statistical  properties  of  the  noise  to  distinguish  the  noise  from  a  maneuver.  Since 
we  assume  that  the  noise  is  zero-mean,  additive,  white,  Gaussian  noise  (AWGN),  we  will 
use  the  property  of  whiteness  and  the  mean  as  test  statistics  for  our  comparison.  The 
following  sections  describe  simple  methods  of  exploiting  these  noise  properties  for  the 
purpose  of  maneuver  detection. 
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1.  Temporal  Correlation 

The  property  of  whiteness  of  a  random  process  requires  that  the  spectral 
distribution  of  that  process  be  flat.  Whiteness  also  requires  that  samples  of  the  process 
taken  at  different  times  be  uncorrelated.  For  Gaussian  processes,  this  leads  directly  to 


50 


the  property  that  samples  taken  at  different  times  are  independent.  Simply  put,  the 
value  of  the  noise  at  a  given  time  is  independent  of  the  value  of  the  noise  at  any  other 
time. 

But  the  same  is  not  true  for  a  target  maneuver.  When  the  target  accelerates 
to  one  side,  we  expect  that  the  observations  will  continue  to  be  off  to  that  side  until  the 
Kalman  filter  recaptures  the  target.  Even  then,  the  observations  will  again  drift  off  to 
the  side  until  the  target  stops  accelerating.  So  the  time  during  which  the  observations 
drift  due  to  target  maneuvering  is  strongly  correlated. 

We  can  exploit  this  difference  in  time  correlation  by  requiring  that  residuals 
exceed  the  maneuver  gate  more  than  just  once  in  a  row  to  be  considered  a  maneuver. 
We  chose  to  require  two  consecutive  gatings  to  signal  a  target  maneuver.  This  provided 
a  compromise  between  fast  response  and  low  false  alarm  rates.  On  the  average,  this 
method  would  reject  50%  of  the  aberrant  noise  signals.  If  we  required  three  consecutive 
gates,  we  would  reject  75%  of  the  aberrant  noise,  but  the  filter  response  would  be 
slowed.  To  improve  gating  performance,  we  chose  to  use  two  consecutive  gates  and  then 
further  process  the  gate  by  requiring  spatial  correlation. 

2.  Spatial  Correlation 

Just  as  the  noise  values  are  uncorrelated  in  time,  so  are  they  uncorrelated  in 
space.  This  is  a  direct  consequence  of  the  noise  being  AWGN  of  zero-mean.  Likewise, 
the  maneuver  observations  are  heavily  correlated  in  space,  and  remain  so  until  the 
maneuver  is  complete.  Using  a  method  similar  to  that  used  for  temporal  correlation,  we 
signal  a  maneuver  only  when  two  consecutive  gates  are  detected  on  the  same  side  of  the 
projected  observation.  This  method  further  reduces  the  false  alarm  rate  by  the  same 
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order  as  that  of  temporal  gating.  AH  that  is  required  now  is  to  determine  the  size  of  the 
gate. 

As  will  be  shown  in  Chapter  VII,  a  nominal  value  for  the  gate  size  is  two. 
This  value  rejects  all  noise  values  which  are  less  than  two  standard  deviations  in 
magnitude.  Since  the  noise  is  Gaussian,  this  will  give  an  average  false  alarm  rate,  of  only 
five  percent.  By  further  processing  with  temporal  and  spatial  correlation,  the  false 
alarm  rate  is  reduced  to  about  one  percent. 
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VII.  SIMULATIONS 


This  chapter  presents  only  the  simulations;  the  results  are  iiscussed  in  the  next 
chapter.  The  simulations  which  are  presented  in  this  chapter  are  in  accordance  with  the 
scenarios  given  in  Table  (4.1)  and  are  given  in  two  sections.  The  first  section  shows  the 
results  of  the  noise  power  estimator  described  in  Chapter  V,  while  the  second  section 
gives  the  results  of  the  maneuver  gating  schemes  described  in  Chapter  VI.  These 
simulations  were  run  using  the  program  code  given  in  Appendix  C. 

The  noise  used  in  the  simulations  was  generated  by  software  which  implemented  a 
pseudo-random  noise  algorithm.  This  noise  sequence  was  then  filtered  to  reduce  the 
effect  of  abnormal  sequences  generated  by  the  algorithm.  To  further  reduce  the  effects 
of  singularly  beneficial  (or  malevolent)  sequences,  the  simulations  were  each  run  for  a 
suite  of  30  independent  noise  sequences.  The  results  given  for  each  scenario  is  the 
average  of  these  30  simulations.  For  the  plots  of  the  target  tracks,  one  noise  sequence 
was  chosen  at  random  to  be  representative  of  the  suite. 

A.  NOISE  ADAPTATION 

Figures  (7.1)  and  (7.2)  show  the  output  of  the  noise  power  estimator  as  a  function 
of  the  input  observation  noise  power  for  the  non-maneuvering  scenarios,  one  through 
three.  The  noise  powers  are  normalized  to  the  values  given  in  Equations  (4.15)  and 
(4.16).  Separate  graphs  are  given  for  the  noise  power  estimates  in  the  bearing  and  range 
dimensions. 

Using  these  estimates  in  the  non-maneuvering  scenarios  results 'ii  the  performance 
shown  in  Figures  (7.3)  through  (7.5).  These  graphs  show  the  relative  errors  of  the 
Kalman  filters  as  the  observation  noise  power  is  scaled  from  its  nominal  values. 
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Relative  Error 


Filter  Performance  with  Noise  Adaptation  for  Scenario  #1 


Figure  7.3:  Filter  Performance  with  Noise  Estimation  for  Scenario  #1 


56 


Filter  Performance  with  Noise  Adaptation  for  Scenario  #2 


Figure  7.4:  Filter  Performance  with  Noise  Estimation  for  Scenario  #2 
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Filter  Performance  with  Noise  Adaptation  for  Scenario  #3 


Figure  7.5:  Filter  Performance  with  Noise  Estimation  for  Scenario  #3 
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B.  MANEUVER  GATING 


The  simulations  for  the  maneuver  gating  performance  are  given  in  five  stages. 
The  first  stage,  Figures  (7.6)  and  (7.7),  shows  the  filter  performance  as  a  function  of  the 
maneuver  gate  size  for  the  maneuvering  and  non-maneuvering  scenarios.  Using  this 
data,  the  second  stage  shows  the  filter  performance  for  scenarios  two  through  five  for 
each  of  the  gating  methods  described  in  Chapter  VI.  These  are  shown  in  Figures  (7.8) 
through  (7.11). 

The  third  stage  of  simulations  shows  the  plot  of  the  target  tracks  and  the  filter 
estimates  for  a  representative  noise  vector  for  each  of  the  gating  methods  using  scenarios 
four  and  five.  These  graphs  are  given  in  Figures  (7.12)  through  (7.17).  The  fourth 
stage,  Figures  (7.18)  through  (7.23),  shows  the  actual  and  expected  errors  for  the  graphs 
given  in  stage  four.  The  fifth  stage  compares  the  actual  errors  for  the  two  gating 
methods  for  the  simulations  run  in  stage  four,  and  is  shown  in  Figures  (7.24)  through 
(7.27). 

The  results  of  these  simulations  and  their  implications  are  discussed  in  the  next 
chapter.  All  conclusions  are  summarized  in  the  final  chapter,  Chapter  IX,  along  with 
recommendations  for  further  investigation. 
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Figure  7.6:  Filter  Performance  as  a  Function  of  Gate  Size  for  Non-Maneuvering 
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Filter  Performance  vs  Mahalanobis  Distance 
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Figure  7.7:  Filter  Performance  as  a  Function  of  Gate  Size  for  Maneuvering  Targets 
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Filter  Performance  with  Gating  for  Scenario  #2 


Figure  7.8:  Filter  Performance  for  Scenario  #2 
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Filter  Performance  with  Gating  for  Scenario  #? 


Figure  7.9:  Filter  Performance  for  Scenario  #3 
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Filter  Performance  with  Gating  for  Scenario  #4 


Figure  7.10:  Filter  Performance  for  Scenario  #4 


64 


Filter  Performance  with  Gating  for  Scenario  #5 
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Figure  7.11:  Filter  Performance  for  Scenario  #5 
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Noise  Ratio  =  10,  Error  =  3.296,  Scenario  #4 


X  (km) 


Figure  7.13:  Target  Track  with  Reset  Covariance  Gating 
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Noise  Ratio  =  10,  Error  =  28.88,  Scenario  #5 
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Figure  7.15:  Target  Track  with  No  Maneuver  Gating 
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Noise  Ratio  =  10,  Error  =  2.798,  Scenario  #5 
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Figure  7.16:  Target  Track  with  Reset  Covariance  Gating 
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Noise  Ratio  =  10,  Error  =  1.973,  Scenario  #5 
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Figure  7.17:  Target  Track  with  Incremental,  Correlated  Gating 
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Actual  and  Expected  Errors  for  Scenario  #4 
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Figure  7.19:  Actual  and  Expected  Errors  with  Reset  Covariance  Gating 
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Figure  7.20:  Actual  and  Expected  Errors  with  Incremental,  Correlated  Gating 
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Actual  and  Expected  Errors  for  Scenario  #5 
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Figure  7.22:  Actual  and  Expected  Errors  with  Reset  Covariance  Gating 
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Actual  Errors  for  Scenario  #4 


Figure  7.24:  Actual  Errors  with  No  Gating  and  with  Incremental,  Correlated  Gating 
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1'igure  7.25:  Actual  Errors  with  No  Gating  and  with  Incremental,  Correlated  Gating 
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Actual  Errors  for  Scenario  #4 


Figure  7.26:  Actual  Errors  with  Reset  Covariance  Gating  and  with  Incremental, 
Correlated  Gating 
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Actual  Errors  for  Scenario  #5 


Figure  7.27:  Actual  Errors  with  Reset  Covariance  Gating  and  with  Incremental, 
Correlated  Gating 
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VIII.  RESULTS 


This  chapter  presents  the  results  of  the  simulations  run  in  Chapter  VII.  Following 
the  structure  of  this  report,  the  results  are  divided  into  two  sections.  The  first  section 
shows  the  results  of  the  noise  power  estimation  algorithm  given  in  Chapter  V,  while  the 
second  section  gives  the  results  of  the  maneuver  adaptation  schemes  described  in  Chapter 
VI. 


A.  NOISE  ADAPTATION 

The  ability  of  the  Kalman  filter  to  estimate  the  noise  power  present  in  the 
observations  is  shown  by  Figures  (7.1)  and  (7.2).  Figure  (7.1)  shows  that,  for  a  non¬ 
maneuvering  target,  the  Kalman  filter  produces  a  noise  power  estimate  for  the  bearing 
noise  which  is  a  linear  function  of  the  actual  noise  power.  This  linear  relationship  is  a 
weak  function  of  the  tracking  geometry  and  holds  for  scenarios  one  through  three.  The 
linearity  constant  can  be  computed  and  used  for  further  processing  to  provide  even 
better  estimates  of  both  the  bearing-noise  power  and  the  target  state.  This  constant  is 
likely  a  strong  function  of  the  state  definition  and  should  be  found  by  simulation  for 
each  Kalman  filter  implementation. 

This  noise  power  estimation  breaks  down,  however,  when  the  range-noise  power  is 
estimated,  as  shown  in  Figure  (7.2).  Although  scenario  one  (stationary  target)  provides 
the  same  linear  relationship  found  in  the  bearing-noise  estimate,  the  estimates  for 
scenarios  two  and  three  provide  little  useful  information  for  adapting  the  Kalman  filter. 

Using  the  noise  power  estimates,  the  Kalman  filter’s  performance  was  tested  and  is 
shown  in  Figures  (7.3)  through  (7.5).  Each  case  shows  that  the  filter’s  performance  was 
improved  by  noise  power  adaptation  as  the  observation  noise  was  increased.  This 
improvement  came  in  spite  of  the  poor  estimates  in  the  range  channel. 
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B.  MANEUVER  GATING 


The  block  model  of  the  adaptive  method  used  is  shown  in  Figure  (8.1).  This 
figure  shows  that  the  adaptive  algorithms  use  the  residual  and  error  covariance 
information  from  the  Kalman  filter  and  produce  observation  noise  power  estimates  and 
maneuver  gating  information. 


a  priori  informitton 


*0,0  initial  State  Estimate 

P0I0  initial  Error  Estimate 

0,  *  Expected  Target  Motion 

R  Observation  Noise 
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I  I  I  I 

I  I  I  I 
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Noise  Power 
Estimates  and 
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t  t  t  t 
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"Tk~ 


State  Estimate:  *k»,k,, 


Error  Estimate  P,.fk», 


Standard  Kalman  Filter 


Adaptive  Mechanism 


^  R*  stouai 

and  Error  Estimate 


Adaptive 

Algorithm 


Figure  8.1:  Block  Model  of  Adaptive  Kalman  Filter 
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The  first  result  found  for  maneuver  gating  is  shown  by  Figures  (7.6)  and  (7.7). 
These  two  graphs  show  that  the  performance  of  the  Kalman  filter  in  tracking  a  non¬ 
maneuvering  target  improves  as  the  gate  size  increases.  This  improvement  reaches  a 
steady  value  at  a  gate  size  corresponding  to  a  Mahalanobis  Distance  of  about  2.25.  But 
>the  filter’s  performance  in  tracking  a  maneuvering  target  improves  as  the  gate  size 
decreases. 

These  two  results  are  in  harmony  with  the  way  in  which  maneuver  gating  operates. 
When  the  target  does  not  maneuver,  any  gating  action  will  only  incorrectly  adjust  the 
error  covariance  matrix  resulting  in  an  improperly  large  weighting  to  be  given  to  the 
current  observation.  This  is  due  to  the  gating  algorithm  incorrectly  detecting  anomalous 
noise  as  target  maneuvering.  When  the  target  does  maneuver,  however,  the  gate  size 
needs  to  be  small  so  that  the  state  estimates  are  able  to  remain  close  to  the  maneuvering 
target.  The  lower  limit  of  this  gate  size  is  a  value  which  is  proportional  to  the  noise 
power  and  inversely  proportional  to  the  magnitude  of  the  target’s  acceleration. 

Using  the  results  given  above,  the  gate  size  used  in  the  simulations  was  set  to  an 
initial  value  of  2.25.  This  value  was  adapted  down  to  a  minimum  of  0.2  whenever  target 
maneuvering  was  detected,  and  reset  to  2.25  when  target  maneuvers  were  complete. 
Adapting  the  gate  size  in  this  fashion  provided  experimentally  optimal  performance  for 
this  set  of  scenarios. 

The  rest  of  the  simulations  show  that  the  overall  best  performance  was  obtained 
for  tracking  maneuvering  targets  by  employing  the  technique  of  incremental,  correlated 
maneuver  gating.  This  method  performed  well  for  both  maneuvering  and  non¬ 
maneuvering  targets,  while  providing  the  greatest  degree  of  noise  rejection.  Reset  gating 
performed  well  for  maneuvering  targets,  but  poorly  for  non-maneuvering  targets.  This 
is  because  reset  gating  tends  to  "chase  after"  noisy  observations  so  that  its  performance  is 
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degraded  in  the  face  of  anomalous  noise.  The  non-adaptive  Kalman  filter  suffered  from 
divergence  in  the  face  of  large  noise  power  and  a  maneuvering  target. 

Figures  (7.18)  through  (7.23)  show  that  the  method  of  incremental,  correlated 
maneuver  gating  also  provided  the  best  estimate  of  its  own  error  performance.  The 
regular  Kalman  filter  clearly  gave  unrealistic  values  for  the  estimated  error,  contributing 
to  its  own  divergence. 
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IX.  CONCLUSIONS  AND  RECOMMENDATIONS 


We  have  shown  that  noise  power  estimation  improves  the  performance  of  an 
extended  Kalman  filter  in  the  face  of  anomalous  observation  noise.  By  estimating  the 
noise  power  from  the  variance  of  the  filter’s  residual  we  adapt  the  filter  to  compensate 
for  varying  noise  power.  Although  the  performance  benefits  were  significant,  much 
work  needs  to  be  done  in  this  area  to  improve  the  noise  power  estimates  further  so  the 
Kalman  can  provide  still  better  state  estimates. 

We  also  introduced  the  method  of  correlated  maneuver  gating  to  adapt  the  Kalman 
filter  to  target  dynamics.  By  ;•  iy  and  temporally  correlating  the  Mahalanobis 
Distance  of  the  residual,  the  Kal.nan  filter’s  performance  is  increased  while  tracking 
tangentially  accelerating  targets.  These  results  should  generalize  to  other  applications  of 
the  extended  Kalman  filter  whose  state  and  observation  spaces  enjoy  a  one-to-one 
mapping. 

Although  we  achieved  our  objectives  of  improving  the  performance  of  an 
extended  Kalman  filter  in  the  face  of  significant  observation  noise  and  target 
maneuvering  through  adaptive  techniques,  there  remains  much  fundamental  work  in  this 
area  for  further  improvements.  Specifically,  we  feel  that  an  approach  incorporating  the 
emerging  potential  of  neural  networks  with  the  mathematical  foundation  of  optimal 
estimation  could  provide  new  and  exciting  insight  to  an  old  problem. 
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APPENDIX  A.  TRANSFORMATION  OF  NOISE  PROCESSES 


The  derivation  of  the  extended  Kalman  filter  equations  assumed  that  the  noise 
processes  inh.  rent  in  the  physical  system  were  zero-mean,  additive,  white,  Gaussian 
noise  (AWGN).  This  is  normally  a  safe  assumption,  but  the  Kalman  derivation  also 
requires  that  the  noise  be  Gaussian  distributed  in  the  state  space.  For  the  system  as 
described  in  Chapter  IV  the  noise  processes  are  defined  in  spaces  other  than  the  state 
space. 

This  appendix  will  show  that  the  transformation  from  the  noise  space  to  the  state 
space  will  result  in  distributions  which  are  Gaussian  for  the  state  excitation  noise,  and 
•approximately  Gaussian  for  the  observation  noise.  Then  the  state  excitation  noise 
covariance  matrix,  Q,  will  be  derived. 

A.  STATE  EXCITATION  NOISE 

For  the  system  used  in  this  report  the  state  is  defined  to  be 


4 


(A.  1) 


where  the  dot  denotes  a  time  derivative  and  the  underbar  denotes  a  matrix  (to  avoid 
confusion  with  the  state,  x).  This  discrete-time  state  consists  of  x  and  y  positions  and 
velocities.  The  noise  vector  for  this  state  is  given  as 


w 


k  “ 


(A.  2) 
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where  the  state  noise  coefficient  matrix  is  defined  as 


r? 


r  - 


T 


0 

0 


0 

0 


T  J 


(A.3) 


and  T  is  the  sampling  interval  of  the  discrete  system. 

The  problem  is  that  the  noise  process  is  described  in  a  coordinate  system  which  is 
aligned  with  the  direction  of  motion.  As  such,  the  noise  elements  describe  the  expected 
linear  and  tangential  accelerations  of  the  target.  We  will  perform  a  linear  transformation 
of  the  Gaussian  noise  into  the  state  space.  [Ref.  5] 

The  velocity  of  the  target  can  be  described  in  terms  of  its  linear  velocity  and 
heading.  This  relationship,  shown  graphically  in  Figure  (A.l),  is  given  as 


and 


vx  «  vtsin9 
vy  «  vtcos0. 


(A.  4) 
(A.  5) 


Taking  the  time  derivative  of  Equations  (A.4)  and  (A.5)  we  get  the  accelerations  in  the  x 
and  v  directions. 


ax  *  vtsin6  +  vt6cos0 
v  .  v 
*  Vtv7  +  vt°v7 

“ 

ay  =  vtcos©  -  vt©sin0 


tv. 


-  v, 


t  V, 


Vtv7  - 


(A.  6) 


(A.  7) 
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Figure  A.l:  Target  Geometry 


The  noise  is  initially  described  by 

E[vt]  «  E{0]  «  0,  (A. 8) 

E(vt2]  *  CTV2,  (A. 9) 

and 

El©2]  -  °q2  (A.  10) 

where  the  standard  deviations  in  the  linear  and  angular  accelerations  are  given  in 
Chapter  IV.  It  is  important  to  note  that  the  random  variables  in  Equations  (A.8)  through 
(A.  10)  are  Gaussian  distributed.  Since  ax  and  ay  are  linear  combinations  of  these 
Gaussian  variables  (by  Equations  (A. 6)  and  (A. 7)),  ax  and  ay  are  also,paussian. 

By  squaring  Equations  (A. 6)  and  (A. 7)  and  taking  the  expectation  of  both  sides, 
recalling  Equation  (A.8),  we  get  expressions  for  the  variances  of  ax  and  ay. 


so  -  ($f)V  ♦  w 

(A.  11) 

EtO  -  v„V 

(A.  12) 

We  also  find  that  the  covariance  of  ax  and  ay  is 

v*vy[[^]  -*©’]• 

(A.  13) 

Armed  with  this,  we  can  proceed  to  calculate  the  state 

excitation  covariance 

matrix,  Q.  The  matrix  Q  was  defined  to  be 

Q  -  E[wkwkTJ. 

(A.  14) 

Substituting  Equation  (A.2)  into  Equation  (A.  14),  we  get 

r  Eta,5)  ^,n  T 

Q  -  r  ,  r  ■ 

L^yl  WlJ 

(A.  15) 

By  applying  Equations  (A. 11)  through  (A. 13)  to  Equation  (A. 15)  we  can  calculate  the 
covariance  of  the  state  excitation  noise  in  the  state  space  itself.  In  addition,  Equations 
(A.6)  and  (A. 7)  ensure  that  the  distributions  are  Guassian. 

B.  OBSERVATION  NOISE 

The  coordinate  transformation  of  the  observation  noise  is  non-linear  due  to  the 
non-linear  re>"*!onship  between  the  observations  and  the  state.  The  x  and  y  positions  of 
the  target  are  relate!  to  the  bearing  and  range  of  the  observations  by 
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and 


x  *  p  sin/9 


(A.  16) 


y  -  p  cos/9  (A.  17) 

where  p  and  /9  are  defined  by  Figure  (A.2). 


Figure  A.2:  Observation  Geometry 


By  starting  with  the  equation  for  the  observations, 

h  -  H(Xk)  +  vk,  (A.  18) 

we  can  describe  the  observations  as 
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where 


and 


(A.20) 


( A.21) 


Putting  Equations  (A.20)  and  (A.21)  into  Equation  (A. 16)  and  expanding  out  the 
trigonometric  functions  [Ref.  6:p.  167]  gives 


For  |/$|  «  1, 
and  for  |?|  «  p0. 


x  *  (P0  *•  P)sin(£0  +  2) 

■  (P0  +  Ti  ( cos£0sin/5  +  sin0ocos?). 

x  «  (P0  +  7>)Oco$pQ  +  sin£0) , 


x  *  p0 sin^0  +  p0/5cos£0  +  7>s'mPQ. 
A  similar  treatment  for  y  results  in 


(A.  22) 

(A.  23) 


(A.  24) 


y  «  p0cos/9q  -  P0?sin/90  +  ?cos/90.  (A.25) 

Since  x  and  y  are  shown  by  Equations  (A.24)  and  (A.25)  to  be  linear  combinations 
of  Gaussian  random  variables,  then  x  and  y  are  also  Gaussian  with  means  and  variances 


ind 


where 


tnd 


Mix]  -  p0 sin0o,  (A.26) 

M[y]  «  P0cos^0,  (A. 27) 

V[x]  -  ( PqCos$) *  sinV0ffp2,  (A. 28) 

VI y]  -  (p0sin tfofi  +  cos Hf}  (A.29) 

Op  -  El?’]  (A. 30) 

o/  -  E#*]  (A. 31) 


This  appendix  has  shown  that  the  noise  parameters  of  the  system  used  in  this 
report  can  be  transformed  into  equivalent  Gaussian  vectors  in  the  state  space.  This  is 
not  only  necessary  for  the  optimality  of  the  linear  Kalman  filter,  but  it  also  gives 
physical  meaning  to  the  error  ellipses  derived  in  Appendix  B. 


APPENDIX  B.  ERROR  ELLIPSES 


Error  ellipses  provide  a  measure  of  confidence  in  the  estimate  of  a  parameter. 
Generally  these  ellipses  indicate  this  confidence  by  visually  displaying  the  smallest 
region  which  has  a  given  probability  of  containing  the  actual  value  of  the  parameter. 
The  size  and  shape  of  the  ellipse  is  dependent  on  the  distribution  of  the  estimate,  which 
is  a  random  variable.  This  appendix  develops  the  equations  used  to  calculate  and  display 
the  error  ellipses  for  a  two  dimensional  Gaussian  error  covariance  matrix  about  a  given 
estimate. 

A.  GAUSSIAN  DISTRIBUTIONS 

A  Gaussian  random  variable,  x,  is  any  random  variable  whose  probability 
distribution  function  is  given  by 


1  ,  -.2 

i  ‘  ‘  x) 

M  -  7m ' 


where  the  mean  and  variance  of  x  are  given  as 


and 


X  =  E[x] 
o2  =  E[x2]. 


The  expectation  operator,  E[],  is  defined  in  Equation  (B.4). 


(B.l) 


(B.2) 

(B.3) 


E(g(x)]  -  f  g(x)  Ax)  dx  (B.4) 

-oc 

.When  the  random  process  is  a  vector  random  process,  then  the  variable  of  interest 
is  a  vector  of  random  variables.  The  Gaussian  probability  density  function  for  a  random 
vector,  X.  is 
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m  Y\  l  -  Jot  -  M)TK'*Qt  -  M) 

AX)  ■  «ce 

(B.5) 

where  the  mean  and  covariance  of  the  vector  X  are  given  as 

M  ■  E(X] 

(B.6) 

and 

K  -  E((X  -  M)(X  -  M)t]. 

(B.7) 

The  probability  density  function  for  a  two-dimensional  random  vector  is  shown  in 
Figure  (B.l).  This  graph  shows  the  probability  curve  to  be  a  two-dimensional  form  of 
the  standard  Gaussian  curve. 


•I 


Figure  B.l:  Two-Dimeusional  Gaussian  Curve 
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The  contour  plot  of  Figure  (B.l)  is  shown  in  Figure  (B.2)  and  plots  contours  of 
constant  probability. 


x 


Figure  B.2:  Contour  Plot  of  Two-Dimensional  Gaussian  Curve 


The  Gaussian  distributions  plotted  in  Figures  (B.l)  and  (B.2)  are  for 


and 


M 


■[;]•[;] 

‘■[in 


(B.8) 


(B.9) 
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B.  TWO  DIMENSIONAL  ERROR  ELLIPSES 


The  most  common  error  ellipse  used  to  indicate  confidence  in  a  position  estimate 
is  a  two  dimensional  representation  of  the  position  error  covariance  matrix.  If  the 
position  state  is  higher  than  second  order,  then  the  state  elements  are  taken  two  at  a  time 
to  provide  for  a  convenient  graphical  result.  The  following  discussion  is  based  upon  a 
position  estimate  of  the  x  and  y  coordinates  of  an  unknown  parameter. 

In  order  to  calculate  the  error  ellipse,  it  is  necessary  to  know  the  mean  and 
covariance,  M  and  K,  of  the  parameter  estimate.  These  matrices  are  defined  by 
Equations  (B.6)  and  (B.7)  for  the  vector 


X-[y]  ( B.  10) 

where  the  underscore  is  used  to  prevent  confusion  between  the  matrix  X  and  the 
coordinate  x. 

In  the  previous  section,  the  probability  density  function  and  probability  contours 
were  plotted  for  a  two-dimensional  Gaussian  random  vector.  The  contour  plot  is  a  locus 
of  all  points  for  which  the  probability  is  a  constant.  This  satisfies  the  following 
equations: 


AX) 


1  -  |(X  -  M)TK*1(X  -  M) 

«i  6 


-  |(X  -  M)^K_1(X  -  M) 
e  *  '-2 

(X  -  M)TK‘j(X  -  M)  -  C 


where  the  constant  C  will  be  determined  shortly. 


(B.I1) 
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In  order  to  separate  the  x  and  y  coordinates  in  Equation  (B.li)  it  is  necessary  that 
K  be  diagonal,  i.e.,  that  the  off-diagonal  terms  of  K  be  zero.  This  is  not  generally  the 
case,  however.  We  must  transform  the  vector  X  into  some  vector  X*  for  which  the 
corresponding  K’  is  diagonal.  This  is  shown  graphically  in  Figure  (B.3),  and  is 
accomplished  by  means  of  eigenvector  transformation. 


Figure  B.3:  Ellipse  Projected  onto  Orthogonal  Coordinates 


1.  Eigenvector  Transformation 

The  eigenvalues,  A,  and  N  x  1  column  eigenvectors,  v,  of  a  square  N  x  N 
matrix,  K,  satisfy 
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(B.  12) 

( B.  13) 

|K  -  AI|  -  0.  (B.14) 

When  Equation  (B.14)  is  expanded  out  it  results  in  a  polynomial  in  A  of  the  same  degree 
as  the  size  of  the  square  matrix  K.  So,  for  an  N  x  N  matrix,  K,  there  exist  N  (not 
necessarily  unique)  solutions  to  Equation  (B.12).  These  solutions  are  the  eigenvalues  of 
K;  the  associated  vectors,  v,  are  the  eigenvectors. 

The  eigenvectors  are  normally  chosen  so  that  they  are  orthonormal.  They 
will  then  satisfy  the  following: 


Kv  -  Av. 


This  can  be  rearranged  to 


(K.  -  AI)v  -  0. 


There  exists  a  non-zero  v  satisfying  Equation  (B.13)  only  if 


viTVj  -  fijj.  ( B.  15) 

When  these  orthonormal  column  vectors  are  combined  into  an  N  x  N  matrix,  V, 

v  *  I  vi  I  v2  I  "•  I  VN  1  <B-16> 

they  give  the  following  useful  result: 

VT  *  V'1.  ( B.  17) 

Combining  Equations  (B.12)  and  (B.16)  we  get  the  first  stage  of  the  transformation. 
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KV  -  K  [  v.  |  Vg  |  -  |  vN  ] 

-  [  Kvj  |  Kv2  |  .-  |  Kvn  ] 

-  [  Vi  |  Ajv2  |  ...  |  Vn  1-  <B>18> 

Applying  a  little  intuition  we  will  now  use  Equations  (B.1S),  (B.17),  and  (B.18)  to  arrive 
at  the  form  of  the  transformation  from  K.  to  K\ 


Defining  K’  to  be  the  diagonal  matrix  of  eigenvalues  of  K,  we  can  write  the 
transformation  of  K  to  K’  in  terms  of  the  eigenvalues  and  eigenvectors. 


(B.20) 


(B.21) 


Putting  Equation  (B.7)  into  (B.21)  we  get  the  corresponding  coordinate  transformation, 
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where 


(B.22) 

(B.23) 

(B.24) 


K*  -  VT  E[(X  -  M)(X  -  M)T]  V 

-  E[VT(X  -  M)(X  -  M)TV] 

-  E(X’X’T], 

X’  -  VT(X  -  M) 

X  «  VX*  +  M. 

Now  that  we  have  a  transformed  coordinate  system  with  a  diagonal  covariance  matrix  we 
can  proceed  to  the  solution  of  the  ellipse  equations. 

2.  Ellipse  Equations 


Using  the  coordinate  system  defined  by  Equations  (B.21)  and  (B.23),  and 
using  Equation  (B.17),  we  can  write  Equation  (B.ll)  as 


(X  -  -  M)  -  C 

(X  -  M)T(VK,VT)*1(X  -  M)  -  C 
(X  -  M^VK’-^HX  -  M)  -  C 
[VT(X  -  MJ^K’^V^X  -  M)]  -  C 

X^K’^X’  «  C  «  c2  (B.25) 

where  we  introduce  the  new  constant  c2  simply  for  later  convenience. 

Since  K’  is  a  diagonal  matrix,  we  can  expand  Equation  (B.23)  into 


where 

and 


/  2 

c 


V 


(B.26) 

(B.27) 

(B.28) 
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Equation  (B.28)  is  the  standard  form  for  the  equation  of  a  two-dimensional  ellipse.  This 
can  be  solved  parametrically  by  dividing  through  by  c3  and  introducing  a  parametric 
variable,  t. 


1 


■  cos2t  +  sin3t 

So  our  parameterized  equations  for  x’  and  y’  become 


and 

where  t  £  [0,2ff]. 


x’  «  Cffx.CO$t 
y’  «  cay,sint 


(B.29) 


(B.30) 

(B.31) 


The  constant  c  gives  the  size  of  the  ellipse  in  terms  of  the  two-dimensional 
standard  deviation  for  the  covariance  matrix,  K\  The  probability  of  a  point  lying  inside 
the  ellipse  is  related  to  c  by 


Pr  -  JJS  /(X)  dxdy  (B.32) 

where  S  is  the  locus  of  points  on  the  surface  inside  the  ellipse  defined  by  Equation 
(B.26)  and  the  function  /(X)  is  defined  by  Equation  (B.5).  The  relationship  between  c3 
and  Pr  for  a  two-dimensional  Gaussian  distribution  is  given  on  page  537  of  Reference  6. 

Solving  Equation  (B.26),  substituting  the  solution  into  Equation  (B.24),  and 
plotting  the  points  yields  an  error  ellipse  denoting  the  probability  specified  by  the  choice 
of  the  constant  c3.  We  have  employed  this  method  for  the  calculation  and  plotting  of 
error  ellipses  in  this  report. 
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APPENDIX  C.  PROGRAM  CODE 


All  of  the  simulations  for  this  project  were  run  on  IBM-PC1  class  computers  using 
the  matrix  manipulation  language  MATLAB2,  version  3.5f.  This  appendix  contains  the 
source  code  for  all  of  the  functions  written  in  support  of  this  project. 

Only  minor  programming  experience  is  required  to  understand  these  files.  While 
MATLAB  is  similar  to  FORTRAN,  MATLAB's  control  structures  are  much  less 
complex.  Comments  are  started  by  the  percent  sign  (%)  and  continue  to  the  end  of  the 
line. 

To  aid  the  reader  in  scanning  and  retyping  these  functions,  each  file  is  started  on 
a  new  page.  Although  an  analysis  of  the  workings  of  these  files  is  not  necessary  to 
understand  this  report,  the  curious  (or  skeptical)  reader  is  highly  encouraged  to  examine 
them  closely. 

The  author  neither  claims  nor  desires  to  hold  any  copyright  privileges  on  the 
source  code.  Written  requests  for  the  source  code  on  computer  disk  should  be  sent 
either  to  the  author,  or  to  Professor  Harold  A.  Titus.  Address  information  can  be  found 
in  the  Initial  Distribution  List  at  the  end  of  this  report. 

All  of  the  files  listed  in  the  second  section  of  this  appendix  provide  general 
support  for  the  main  files  listed  in  the  first  section.  These  support  files  are  not  specific 
to  the  simulations  run  for  this  report,  but  can  be  used  for  a  variety  of  purposes. 


1  IBM  and  IBM-PC  are  registered  trademarks  of  IBM. 

2  MATLAB  is  a  registered  trademark  of  The  MathWorks,  Inc.. 
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A.  INCLUDE. M 


%  This  file  is  a  collection  of  constants  which  are  used 
%  by  multiple  files. 

%  put  the  following  line  in  the  files  which  use  this: 

% 

%  load  include.mat 

%  Stephen  L.  Spehn  30  Jan  1990 

%  YES  and  NO  are  defined  so  as  to  index  into  YN_STR 

NO  -  1; 

YES  -  2; 

YN_STR  «  [  ’NO  ’ 

’YES’  ]; 

LEFT  -  1; 

RIGHT  -  2; 

TIME_INTERVALS  -  32; 

DELTA__T  «  10;  %  10  seconds 


save  include.mat; 


B.  DRIVER.M 


%  Stephen  L.  Spehn  31  Jan  1990 

%  This  file  calls  SENRUN01  with  a  variety  of  values  for  the 
%  following  parameters: 

% 

%  MD  Mahalanobis  Distance 

%  RAI  Residual  Adaptation  Interval 

%  OCS  Observation  Covariance  Scale 

%  ONV  Observation  Noise  Variance 

%  SG  Spatial  Gating 

%  SN  Scenario  Number 

%  META__T  Make  track  graphs 


% 

META_R 

Make  rest  of  the  graphs 

% 

%  Number  xO 

yo 

vxO 

vyO 

g 

Duration 

% 

(km) 

(km) 

(km/hr) 

(km/hr)  (g’s) 

(sec) 

% 

% 

1  5 

37 

0 

0 

0 

0 

% 

2  5 

37 

450 

0 

0 

0 

% 

3  5 

37 

400 

-280 

0 

0 

% 

4  5 

37 

600 

50 

0.8 

40 

% 

5  5 

37 

450 

-300 

-0.8 

50 

% 

%  Target  tracks  for  these  scenarios  are  in  the  files 
%  scen_l.mat  through  scen_5.mat. 

!del  send* 

!del  sen02.* 

%  Recompile  included  files 
include, clear; 
noise,clear; 
scen_gen, clear; 
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%  DEFINED  Constants 
load  include.mat; 


MD  -  [  .75  1  1.25  1.5  1.75  2  2.25  2.5  ]; 

OCS  -  [  1  ]; 

ONV  -  [  0.5  1  2  5  10  i 
RAI  -  (  TIME_INTERVALS  ]; 

SG  -  [  YES  ]; 

SN  -[  1  2  3  4  5  ]; 

META_T  -  NO; 

META_R  -  NO; 

for  nl  «  l:Iength(SN) 
for  n2  -  l:length(MD) 

senrund(MD(n2),RAl,OCS,ONV,SG,SN(nl),META_T,META_R); 

end; 

end; 

Icopy  senOl.*  sen02.* 

!del  send  * 


MD  «  [  0  ]; 

OCS  -  [  1  ]; 

ONV  -  [  0.1  0.2  0.35  0.5  0.75  1  2  3.5  5  7.5  10  ]; 

RAI  -  [  0  TIME_INTERVALS  ]; 

SG  «  [  NO  ]; 

SN  -[  1  2  3  4  5  ]; 

META_T  -  NO; 

META_R  -  NO; 

for  nl  -  l:length(SN) 
for  n2  *  l:length(RAI) 

senrunOl(MD,RAI(RAI),OCS,ONV,SG,SN(nl),META_T,META_R); 

tnd; 

end; 

exit 
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C.  SENRUN01.M 


function  senrun01(MD,RAI,OCS,ONV,SG,SN,META_T,META_R) 

%  Stephen  L.  Spehn  31  Jan  1990 

%  This  is  the  driver  file  for  the  sensitivity  test  for  the  extended 
%  Kalman  filter.  The  cumulative  errors  are  shown  as  a  function  of  noise 
%  parameters  for  the  filter. 

% 

%  This  file  calls  the  following  special  functions: 

% 

%  BR_OBS 
%  ERRELLIP 
%  INT2STR2 
%  KF_BR 
%  KF_ERR 
%  RESET 
%  TIMESTR 


%  DEFINED  Constants 
load  include.mat; 


if  nargin  ■*  0 

MD  *  0;  %  Mahalanobis  Distance 

RAI  «  TIME_INTERVALS;  %  Residual  Adaptation  Interval 

OCS  ■  1;  %  Observation  Covariance  Scale 

ONV  «  {  0.2  0.5  1  2  3.5  5  7.5  10  ];  %  Observation  Noise  Variance 

SG  *  YES;  %  Spatial  Gating 

SN  ■  3;  %  Scenario  Number  ’ 

META_T  «  YES;  %  Make  meta  for  tracks 

META_R  »  NO;  %  Make  rest  of  meta  files 

elseif  nargin  -■  8 

error(’Incorrect  number  of  arguments  to  SENRUN01.’); 
end; 


1 
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%  House-cleaning 
reset; 


%  Constants  (Change  to  suit  the  problem.) 


Erase_All_Files 
Make_Mat 
>  Make_Diary 
Make_Data 
Meta_Filename 
Mat_Filename 
Diary_Filename 
Data  Filename 


NO; 

•  NO; 

•  NO; 

-  YES; 

■  ’senOl.met’; 
» ’senOl.mat’; 

•  ’senOl.dia’; 

•  ’sen01.dat’; 


Calculate_XO  « YES; 


Iteration_Limit 

-3; 

% 

Iterated  Kalman 

Iteration_Tolerance 

■  2; 

% 

km 

Ellipse_Size 

-2; 

Ellipse_Type 

-  l; 

% 

0  Probability,  1  Mahalanobis  Distance 

Ellipse_Points 

-20; 

Ellipse_Interval 

■  8; 

%  Calculated  Constants  (Do  not  change  these.) 
Num_jSens_Jluns  ■  length(ONV); 


if  Erase_All_Files  »  YES 
eval([’!del  \Meta_Filename]); 
eval((’!del  \Mat_Filename]); 
eval([*!del  \Diary_Filename]); 
eval([’!del  ’,Data_Filename]); 
end; 

if  Make_Diary  ««  YES 
eval([’diary  \Mat_Filename]); 
end; 
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%  System  Information,  in  continuous  time,  for  target,  and  initial  guess 
A-[0  1  00  %  x 

0  0  0  0  %  xdot 

0001  %  y 

0  0  0  0  ];  %  ydot 

BW  -  l  0  0 
1  0 
00 
0  1]; 

xO  -  [  j  %  Estimate  of  target  initial  state 

0 
37 
0]; 

%  System  disturbance  and  observation  noise  matrices. 

%  The  observation  noise  matrix  will  be  multiplied  by  the  values 
%  in  the  vector  Observation_Noise__Var. 

W  «  [  0.001  0  %  linear  acceleration 

0  0.001  ];  %  angular  acceleration 

V  «  l  0.0005  0.0  %  bearing 

0.0  0.0005  ]*OCS;  %  range 

%  Initial  Error  Covariance  Matrix 
P0  -  [  200  0  0  0 

0  250000  0  0 

0  0  200  0 

0  0  0  250000  ]; 

%  Read  in  the  observation  and  target  tracks,  and  the  time  vector 
eval([’load  scen_’,int2str(SN),*;’]); 

%  Generate  noiseless  observation  matrix, 
obs  -  br_obs(xo,xt); 

%  Generate  noisy  observations, 
load  noise.mat; 

for  nl  «  l:Num_Noise_Runs; 
alstr  «  int2str(nl); 

eval([’temp__n  «  obs_noise’,n  I  str, ’;’]); 
temp_n(!,:)  »  temp_n(l,:)  *  sqrt(V( !,!)); 


temp_.n(2,:)  «  temp_n(2,:)  *  sqrt(V(2,2)); 
evai(fobs_noise\n  1  str,*  ■  temp_n;’]); 
end; 

%  Run  the  sensitivity  analysis 
IL  ■  Iteration__Limit; 

IT  -  Iteration_Tolerance; 

IE  •  Calculate_XO; 
total_err  ■  zeros(Num_Sens_Runs,l); 
res_vars  •  zeros(Num_Sens_Runs,2); 
res_means  «  zeros(Num_Sens_Runs,2); 

%  For  each  noise  variance: 
for  nl  ■  l:Num_Sens_Runs; 
n_var  ■  ONV(nl); 

clc, 

fprintf(’This  computer  is  busy\n\n’); 
fprintf(’Scenario  Number  «  %1.0f\n\SN); 
fprintf(’MahaIanobis  Distance  »  %4.2f\n\MD); 
fprintf(’ Adaptation  Interval  ■  %1.0f\n\RAl); 
fprintf(’Covariance  Scale  »  %4.2f\n\OCS); 

fprintf([’Spatial  Gating  -  \YN_STR(SG,:),’\n\n’]); 

fprintf(’Noise  Power  Ratio  «  %4.2f\n\n__var); 

for  n2  «  l:Num_Noise_Runs; 

eval([’obs_noise  «  obs_noise\int2$tr(n2),’  *  sqrt(n_var);’]); 
n_obs  *  obs  +  obs_noise; 

%  Get  innovations,  xy  covariance,  and  estimates. 
[Z,GI,Res,Pxy,xe]  «  kf_br(xo,n_obs,xO,IE,... 

A,BW,W,V,DTS/3600,MD,P0,RAI,IL,IT,SG); 

total_err(nl,l)  ■  total_err(nl,l)  +  kf_err(xe,xt, Time/3600); 
res_vars(nl,:)  *  res_vars(nl,:)  +  diag(cov(Res’))’; 
res_means(nl,:)  «  res_means(nl,:)  +  mean(Res’); 
end; 
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eval(['GI\int2str(nl ),’  -  GI;’]); 
evel({’Res\int2str(nl),* «  Res;’]); 
eval({’Pxy\int2str(nl),’  •  Pxy;’]); 
eval([’xe\int2str(nl),’ »  xe;']); 

%  Plot  track 
if  META  JT  —  YES 
n4  ■  0; 

for  n3  -  2:length(xe); 
if  rem(n3,EUipse_lnterval)  -■  1 
n4  ■  n4+l; 

Pxyl  -  Pxy(:,2*n3-l:2*n3); 

[xp,yp] «  errellip([xe(i,n3);xe(3,n3)],Pxyl,Ellipse_Size,... 
Ellipse_Points,Ellipse_Type); 

eex(:,n4)  ■  xp; 
eey(:,n4)  -  yp; 
end; 
end; 

axis(’square’); 
axis([0  50  0  50]); 

plot(xt(l,:),xt(3,:),’o’,xe(l,l:n3),xe(3,l:n3), 
xo(  1  ,:),xo(3,:),’x\eex,eey,’-w*); 
grid; 

title([’Noise  Ratio  «  ’,num2str(n_var),... 

Error  *  \num2str(total_err(nl,l)/Num_Noise_Runs),... 
RAI  -  \int2str(RAI),... 

\  SG  -  \YN_STR(SG,:)]); 
xlabeK’X  (kmyj.ylabeK’Y  (km)’); 
eval([*meta  \Meta_Filename]); 
end; 

end; 

res_vars  ■  res_vars  /  Num_Noise_Runs; 
res_means  «  res_means  /  Num_Noise_Runs; 
total_err  «  total_err  /  Num_Noise_Runs; 
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%  Plot  the  results 

if  META__R  ■■  YES  &  Num_Sens_Runs  >  1 
axis(’normal’);axis([l  2  3  4]);axis; 
semilogx(ONV,total_err); 
grid; 

xlabel(’Normalized  Observation  Noise’), ylabeK’Scaled  Total  Error*); 
title([*Sensitivity  of  Extended  Kalman  Filter’,... 

RAI  ■  \int2str(RAI),... 

SO  -  \YN_STR(SG,:)]); 
eval(['meta  \Meta_Filename]); 


%  Display  the  variance  of  the  residual  versus 

%  the  variance  of  the  observation  noise. 

axis(’square’); 

plot(ONV*V(l,l),Z(l,l)); 

grid; 

xlabel('Observation  Noise  Variance’), ylabeI(’Variance  of  the  Residual’); 
title([’Residual  Variance  for  Bearing’,... 

*,  RAI  ■  *,int2str(RAl),... 

’,  SG  -  \YN_STR(SG,:)]); 
eval([’meta  ’,Meta_Filename]); 

plot(ON  V*  V(2,2),Z(2,2)); 
grid; 

xlabeK’Observation  Noise  Variance’), ylabeK’Variance  of  the  Residual’); 
title([’Residual  Variance  for  Range’,... 

’,  RAI  *  \int2str(RAl),... 

’,  SG  -  ’,YN_STR(SG,:)]); 
eval([’meta  ’,Meta_Filename]); 
end; 


%  Plot  the  total  errors  as  a  function  of  observation 
if  META_T  —  YES 

x_max  -  ceil((TIME_INTERVALS+l)/5)*5; 
axis(’normal’);axis([0  x_max  0  15]);axis; 
for  nl  «  Num_Sens_Runs:-l:l 
eval([’xe  ■  xe’,int2str(nl ),’;’]); 
eval([’GI  -  GI’,int2str(nl ),’;’]); 
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rms_err  -  sqrt(  (xe(l,:)  -  xt(l,:)).A2  +  (xe(3,:)  -  xt(3,:)).A2  ); 
plot([0:length(xe)- 1  ],rms_err,0,0); 
hold  on; 

for  n2  «  l:length(GI) 
if  GI(n2)  —  YES 

plot(n2- 1  ,rms_err(n2),’o’); 
end; 
end; 
end; 

hold  off; 
grid; 

xlabel(’Observation  Number’), ylabel(’Error’); 
title([’RMS  Errors  For  All  Runs’,... 

SG  -  \YN_STR(SG,:)J); 
eval([’meta  \Meta_Filename]); 
end; 

%  Save  everything 
if  Make_Mat  »»  YES 
eval([’save  \Mat_Filename]); 
end; 


%  Write  the  results  to  the  data  file, 
if  Make_Data  «»  YES 
if  ~exist(Data_Filename) 

fprintf(Data_Filename, ’Thesis  Data  for  Stephen  L.  Spehn\n’); 
fprintf(Data_Filename,[’Time:  ’,timestr,’\n\n’]); 
else 

fprintf(Data__Filename,’\n\n’); 

end; 

fprintf(Data_Filename, ’Scenario  Number  ■  %1.0f\n’,SN); 

fprintf(Data_Filename,’Mahalanobis  Distance  *  %4.2f\n’,MD); 
fprintf(Data_Filename,’ Adaptation  Interval  «  %1.0f\n’,RAI); 
fprintf(Data_Filename, ’Covariance  Scale  *  %4.2f\n’,OCS); 

fprintf(Data_Filename,[’Spatial  Gating  *  ’,YN_STR(SG,:),’\n\n’]); 

fprintf(Data_Filename,... 

’Noise  Factor  Error  Residual  Mean  (B/R)  Residual  Var  (B/R)\n\n’); 


for  nl  ■  l:Num_Sens_Runs; 

f printf(Data_Filename,’  %6.2f  %6.2r,ONV(nl),total_err(nl,l)); 
if  res_means(nl,l)  >»  0 
fprintf(Data_Filename,’  ’); 
end; 

fprintf(Data_Filename,’  %6.4f\res_means(nl ,  1 )); 
if  re$_means(nl,2)  >•»  0 
fprintf(Data_FiIename,'  ’); 
end; 

fprintf(Data_Filename,’  %6.4f\re$_means(nl,2)); 

if  res_vars(nl,l)  >»  0 
fprintf(Data_Filename,’  ’); 
end; 

fprintf(Data_Filename,’  %6.4f\Z(  1,1)); 
if  res_vars(nl,2)  >■  0 
fprintf(Data_Filename,'  ’); 
end; 

fprintf(Data_Filename,’  %6.4f\n\Z(2,2)); 
end; 
end; 

%  Cleanup 
diary  off; 
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D.  KF  BR.M 


function  [Z,GI,Res,Pxy,xe]  ■  kf_br(xobs,zobs,xO,IE,A,B,W,V,T,MD,PO,RAI,IL,IT,SG) 
% 

%KF_BR 

%  [Z,GI,Res,Pxy,xe]  «  kf Jjr(xobs,zobs,xO,IE,A,B,W,V,T,MD,PO,RAI,IL,IT,SG) 

%  takes  the  following  arguments: 

% 

%  xobs  vector  of  observer  positions 

%  zobs  matrix  of  no»sy  observations 

%  xO  initial  estimate 

%  IE  Initial  Estimate:  0  «  use  xO;  1  «  use  zobs(l) 

%  A  continuous  time  state  dynamics  matrix 

%  B  continuous  time  state  excitation  matrix 
%  W  state  forcing  function  covariance  matrix 

%  V  observation  noise  covariance  matrix 

%  T  vector  of  observation  intervals;  these  times  are  not  running 
%  times,  but  interval  times 

%  MD  Mahalanobis  Distance  for  maneuver  gating:  MD  -**  0  means  no  gating 
%  PO  initial  error  covariance  matrix 

%  RAI  Residual  Adapting  Interval:  RAI  **  0  means  no  adaptation 
%  The  observation  noise  variance  is  compensated  for  the  variance 

%  of  the  residual  at  these  intervals. 

%  1L  Iteration  Limit 

%  IT  Iteration  Tolerance 

%  SG  Spatial  Gating:  1  for  Yes,  0  for  No 

% 

%  This  function  returns  a  row  vector  of  gating  information, 

%  an  augmented  column  of  innovations,  an  augmented  row  of  error 
%  covariance  matrices,  and  an  augmented  row  matrix  of  estimated  states. 

%  Stephen  L.  Spehn  30  Jan  1990 

%  Check  arguments 
if  nargin  —  15 

error(Tncorrect  number  of  arguments  to  KF_BR.’); 
end; 
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%  DEFINED  Constants 
load  include.mat; 

%  Allocate  variables 
kmax  -  length(T)+l; 

Res  ■  zeros(2,kmax-l); 

Pxy  »  zeros(2,kmax*2); 

Pxy(:,l:2)  «  £  P0(1,1)  P0(l,3) 

P0(3,l)  PO(3,3)  ]; 
xe  «  zeros(length(xO),kmax); 
if  IE  «  NO 
xe(:,l)  -  xO; 
else 

xe(l,l)  *  xobs(l,l)  +  zobs(2,l)  *  sin(zjbs(l,l)); 
xe(3,l)  ■  xobs(3,l)  +  zobs(2,l)  *  cos(zobs(l,l)); 
end; 

MDSide  «  NO  *  ones(l,kmax);  %  Side  of  the  residual  (LEFT  or  RIGHT) 
GI  *  NO  *  MDSide;  %  Gating  output 

MDR  «  0;  %  Mahalanobis  Distance  Ratio  of  the  Residual 

xi  «  zeros(length(xO),IL+l); 

I  -eye(PO); 

P  *P0; 

KP  *  1; 
if  SG  «  NO 
MD  =  0; 
end; 


%  Kalman  Filter  loop 
next_adjust  =  RAI; 
if  RAI  ~=  0 
Adapt  =  NO; 
else 

Adapt  *  SG; 
end; 

oldt  =  T(l); 
k  -  0; 
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while  1 
k  »  k  +  1; 
if  k  ««  kmax 
break; 
end; 

if  k  «  1 
P«  PO; 
end; 

kbest  «  max(k,next_adjust-RAI); 
if  (T(l)  —  oldt)  |  (k  —  1) 

[phi, del]  -  c2d(A,B,T(l)); 
end; 

oldt «  T(k); 

%  Time  update  of  estimate 
xeminus  *  phi*xe(:,k); 
xi(:,l)  *  xeminus; 

%  Do  the  iterated  Kalman  equations 
for  kl  «  1:IL 
PI  -  P; 

%  Transform  from  polar  W  to  rectangular  Q 
varv  *  W(l,l); 
varth  -  W(2,2); 

vt  *  sqrt((xi(2,kl))A2  +  (xi(4,kl))A2); 
if  vt  0 

qll  =  varv  *  (xi(2,kl)  /  vt)A2  +  varth  *  (xi(4,kl))A2; 
q22  *  varv  *  (xi(4,kl)  /  vt)A2  +  varth  *  (xi(2,kl))A2; 
ql2  ■  xi(2,kl)  *  xi(4,kl)  *  (varv  /  vtA2  -  varth); 
q21  ■>  q  1 2; 

Q  -  [  qll  ql2 

q21  q22  ]; 
else 

Q  *  zeros(W); 
end; 

Q  =  dei*Q*(del’); 
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%  Linearize  H  about  best  estimate 

deltx  -  xi(l,kl); 

deity  «  xi(3,kl); 

r2  «  de»txA2  +  deltyA2; 

r  »  sqrt(r2); 

H  ■  [  delty/r2  0  -deltx/r2  0 
deltx/r  0  delty/r  0  ]; 

resid  *  br_resid(zobs(:,k+l),br_obs(xobs(:,k+l),xi(:,kl))); 
MDSide(k+l)  «  br_side(xobs(:,k+l),xi(:,kl),zobs(:,k+l)); 

KP  «  1; 

MD1  —  NO; 
while  1 

P2  *  KP*P1; 

P2  «  phi*P2*phi’  +  Q; 
vresid  «  H*P2*H’  +  V; 

G  -  P2*H’  /  vresid; 

P2  -  (I  -  G*H)*P2; 


%  Projected  error  covariance 
%  Expected  variance  of  the  residual 
%  Kalman  gain 
%  updated  error  covariance 


%  Now  check  the  Mahalanobis  Distance  of  the  residual 
if  MD  «  0 
MDR  -  0; 
else 

MDR  *  (resid’  /  vresid  *  resid)  /  MDA2; 
end; 

if  MDR  >  1 
MD1  =  YES; 
else 

MD1  =  NO; 
end; 

if  (MD  «  0)  |  (MD1  «  NO)  1  (Adapt  —  NO)  ... 

1  ((SG  «  YES)  &  (MDSide(k)  MDSide(k+l))) 
xi(:,kl+l)  *  xeminus  +  G*(resid  -  H*(xeminus  -  xi(:,k  1 ))); 
break; 
else 

KP  *  1.2*KP; 

GI(k+l)  -  YES; 
end; 
end; 


1  IS 


%  Check  iteration  tolerance 

if  ( (xi(l,kl+l)-xi(l,kl))A2  +  (xi(3,kl+l)-xi(3,kl))A2  )  <  ITA2 
break; 
end; 
end; 

%  Estimate  is  final  iterated  estimate 
'  xe(:,k+l)  -  xi(:,kl+l); 

P  *  P2; 

Res(:,k)  -  resid; 

Pxy(:,(2*k+ 1 :2*k+2))  «  [  P(l,l)  P(l,3) 

P(3,l)  P(3,3)  ]; 

%  Update  the  a  priori  observation  noise  covariance  based  on  the  residu  tls. 
%  and  start  from  the  first  observation, 
if  k  ««  next_adjust 
V  ■  cov(Res’); 

next_adjust  «  next_adjust  +  RAI; 

Adapt  *  YES; 
k  -  0; 
end; 

end; 

Z  *  V; 
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£.  BR  OBS.M 


function  obs  *  br_obs(x_obs,x_tar) 

% 

%BR_OBS 

% 

%  This  function  takes  a  vector  of  observer  positions 
%  and  a  vector  of  target  positions  and  returns  a  matrix 
%  of  observation  vectors  consisting  of  the  bearing  and  range 
%  from  the  observer  to  the  target.  The  bearings  are 
%  given  in  radians. 

% 

%  x_obs  vector  of  observer  positions, 

%  x_tar  vector  of  target  positions. 

% 

%  obs  matrix  of  bearing/range  observation  vectors 

%  Stephen  L.  Spehn  21  Nov  1989 

%  Check  input  arguments 
if  nargin  2 

error(Tncorrect  number  of  arguments  to  BR_OBS.’); 
end; 

%  Get  the  x  and  y  differences  and  calculate  observations 
x  =  x_tar(l,:)  -  x_obs(l,:); 
y  *  x_tar(3,:)  -  x_obs(3,:); 

b  =  atan2(x,y); 
r  =  sqrt(x  .A2  +  y  .A2); 

obs  *  [  b 

r  1; 
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F.  BR  RESID.M 


function  resid  «  br_resid(z_obs,z_est) 

% 

%BR_RESID 

% 

%  resid  «  br_resid(z_obs,z_est)  takes  the  following  arguments: 
% 

%  z_obs  observation  vector, 

%  z_est  estimated  observation  vector. 

% 

%  This  function  returns  a  vector  of  residuals  for  use  in  the 
%  Kalman  filter  estimate  update  equation. 

%  Stephen  L.  Spehn  18  Dec  1989 

%  Check  number  of  input  arguments 
if  nargin  2 

error(’Incorrect  number  of  arguments  to  BR_RESID.’); 
end; 

%  Get  difference 
resid  *  z_obs  -  z_est; 

%  Normalize  by  putting  angle  in  [-pi. .pi] 
resid(l,:)  =  resid(l,:)  +  2*pi*(resid(l,:)  <  -pi); 
resid(  1 ,:)  =  resid(  1 ,:)  -  2*pi*(resid(l,:)  >  pi); 
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G.  BR  SIDE.M 


function  side  «  br_side(xo,xe,z); 

% 

%side  *  br_side(xo,xe,z); 

% 

%  This  function  takes  the  observer’s  position,  the  current 
%  target  estimate,  and  the  bearing/range  observation  and  returns 
%  an  integer  indicating  on  which  side  of  the  estimate  the 
%  the  observation  lies. 

% 


% 

xc 

observer  location 

tx 

% 

vx 

% 

y 

% 

vy  ] 

% 

% 

xe 

target  estimate 

[  x 

% 

vx 

% 

y 

% 

vy  ] 

% 

% 

z 

observation 

[  bearing 

% 

range  ] 

%  Stephen  L.  Spehn  30  Jan  1990 

%  Check  the  input  arguments 
if  nargin  3 

errorflncorrect  number  of  arguments  to  BR_SIDE.’); 
end; 

%  DEFINED  Constants 
load  include.mat; 

%  Get  tne  x  and  y  difference  between  the  estimate  and  the  observation 
xdiff  =  xo(l)  +  z(2)  *  sin(z(  1 ))  -  xe(l); 
ydiff  =  xo(3)  +  z(2)  *  cos(z(l))  -  xe(3); 
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%  Rotate  the  velocity  vector  of  the  estimate  by  pi/2 
xr  -  rotate_v([xe(2);xe(4)],pi/2); 

%  Get  the  component  of  the  difference  vector  along  this  rotated  vector 
sign_r  ■  xr(l)  *  xdiff  +  xr(2)  *  ydiff; 

%  If  this  is  positive,  then  the  observation  lies  to  the  right 
if  sign_r  >  0 
side  -  RIGHT; 
else 

side  *  LEFT; 
end; 


H.  SCEN  GEN.M 


function  scen_gen(n) 

%  Stephen  L.  Spehn  31  Jan  1990 


%  This  file  generates  target  tracks  for  the  various  scenarios 
%  and  stores  them  in  mat  files. 

% 


%  Number  xO 

y0 

vxO  vyO 

8 

Duration 

% 

% 

(km) 

(km) 

(km/hr)  (km/hr)  (g’s) 

(sec) 

% 

1 

5 

37 

0  0 

0 

0 

% 

2 

5 

37 

450  0 

0 

0 

% 

3 

5 

37 

400  -280 

0 

0 

% 

4 

5 

37 

600  50 

0.8 

40 

% 

% 

5 

5 

37 

450  -300 

-0.8 

50 

% 

% 

Target  tracks  for  these  scenarios  are 
scen_l.mat  through  scen_5.mat. 

in  the  files 

%  DEFINED  Constants 
load  include.mat; 


%  Calculate  time  vector 

DTS  «  ones(  1  ,TIME_INTER  VA  LS)*DELTA_T; 
Time  =  zeros(l,TIME_INTERVALS+l); 
for  nl  ■  2:length(Time); 

Time(nl)  -  Time(nl-l)  +  DELTA_T; 
end; 

xo  *  zeros(4,TIME_INTERVALS+l); 
xt  *  xo; 

A  *  [  0  1  0  0  %  x 

0  0  0  0  %  xdot 

0001  %  y 

0  0  0  0  ];  %  ydot 
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%  scen_l  .mat 

xtO  -  [  5;  0;  37;  0  ]; 

xt  ■  track_t(xtO,DTS/3600,A); 

save  scen_l.mat  xo  xt  Time  DTS; 

%  $cen_2.mat 
xtO  -  [  5;  450;  37;  0  ]; 

,  xt  ■  track_t(xt0,DTS/3600,A); 
save  scen_2.mat  xo  xt  Time  DTS; 

%  scen_3.mat 

xtO  «  [  5;  400;  37;  -280  ]; 

xt  «  track_t(xt0,DTS/3600,A); 

save  scen_3.mat  xo  xt  Time  DTS; 

%  scen_4.mat 
g  «  9.8  *  3600A2  /  1000; 
xtO  «  [  5;  600;  37;  50  ]; 

G4  *  0.8; 

ACC_TIME  -  4; 

n  -  length(DTS); 

ml  «  round(n/2); 

m2  *  n  -  ml  -  ACC_TIME; 

xt  »  track__t(xt0,DTS(l;ml-l)/3600,A); 

V  *  sqrt(xt(2,ml)A2  +  xt(4,ml)A2); 
theta  *  atan2(xt(2,ml),xt(4,ml)); 

ACC  =  G4*g; 

da  *  ACC  *  DELTA_T  /  3600  /  V; 

R  «  abs(V*V  /  ACC); 
if  ACC  >  0 

xR  =  xt(l,ml)  +  R*cos(theta); 
yR  »  xt(3,ml)  -  R*sin(theta); 
alphaO  ■  1.5*pi  +  theta; 
else 


%  g  acceleration  in  km/hrA2 


%  1  ..  ml 
%  V  in  km/hr 

%  angle  change  in  radians 
%  R  in  km 
%  right  turn,  Clyde 

%  left  tern 


e: 


xR  =  xt(l,ml)  -  R*cos(theta); 
yR  «  xt(3,ml)  +  R*sin(theta); 
alphaO  =  0.5*pi  +  theta; 


%  1  ..  ml  +  ACC  TIME 


for  nl  -  1:ACC_TIME 
alpha  -  alphaO  +  da*nl; 
xt(l,ml+nl)  ■  xR  +  R*sin(alpha); 
xt(3,ml+nl)  ■  yR  +  R*cos(alpha); 
xt(2,ml+nl)  «  V*cos(alpha); 
xt(4,ml+nl)  ■  -V*sin(alpha); 
end; 

xt  -  [  xt  track_t(xt(:,ml+ACC_TIME),DTS(ml+ACC_TIME+l:n)/3600,A)  ]; 
save  scen_4.mat  xo  xt  Time  DTS; 

%  scen_5.mat 

xtO  -  [  5;  450;  37;  -400  ]; 

G5  -  -0.8; 

ACC_TIME  «  5; 
n  «  length(DTS); 
ml  -  round(n/2); 
m2  *  n  -  ml  -  ACC _ TIME; 

xt  «  track_t(xtO,DTS(l:ml-l)/3600,A);  %  1  ..  ml 

V  »  sqrt(xt(2,ml)A2  +  xt(4,ml)A2);  %  V  in  km/hr 

theta  ■  atan2(xt(2,ml),xt(4,ml)); 

ACC  -  G5*g; 

da  *  ACC  *  DELTA_T  /  3600  /V;  %  angle  change  in  radians 

R  -  abs(V*V  /  ACC);  %  R  in  km 

if  ACC  >0  %  right  turn,  Clyde 

xR  «  xt(l,ml)  +  R*cos(theta); 
yR  «  xt(3,ml)  -  R*sin(theta); 
alphaO  «  1.5*pi  +  theta; 

else  %  left  tern 

xR  ■  xt(l,ml)  -  R*cos(theta); 
yR  ■  xt(3,ml)  +  R*sin(theta); 
alphaO  -  0.5*pi  +  theta; 

V  «  -V; 
end; 

for  nl  «  1:ACC_TIME  %  1  ..  ml  +  ACC_TIME 

alpha  =  alphaO  +  da*nl; 
xt(l,ml+nl)  *  xR  +  R*sin(alpha); 
xt(3,ml+nl)  *  yR  +  R*cos(alpha); 
xt(2,ml+nl)  *  V*cos(alpha); 
xt(4,ml+n!)  =  -V*sin(alpha); 
end; 
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xt  -  [  xt  track__t(xt(:,ml+ACC_TIME),DTS(ml+ACC_TIME+l:n)/3600,A)  ]; 
save  scen_5.mat  xo  xt  Time  DTS; 
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I.  TRACK  T.M 


function  x  -  track_t(xO,tvec,A) 

% 

%TRACK_T 

% 

%  x  -  track_t(xO,tvec,A)  takes  the  following  arguments: 

% 

%  xO  initial  target  position 

%  tvec  vector  of  observation  intervals;  these  times  are  not  running 
%  times,  but  interval  times,  i.e.  tvec  ■  [1  1.5  .2  .7  .9  .5  1] 

%  A  continuous  time  state  transition  matrix 
% 

%  This  function  returns  a  matrix  of  the  target  states. 

%  Stephen  L.  Spehn  15  Nov  1989 

%  Check  arguments 
if  nargin  3 

error(Tncorrect  number  of  arguments  to  TRACK._T.’); 
end; 

%  Check  for  consistency  in  the  size  of  the  inputs 
[rx,cx]  *  size(xO); 
if  cx  1 

error(’xO  must  be  n  x  1,'); 
end; 

[rA,cA]  «  size(A); 
if  rA  cA 

error(’A  must  be  square.’); 
end; 

if  rx  cA 

error(’xO  and  A  must  be  conformable.’); 
end; 

%  Allocate  and  initialize  x  and  B 
N  -  length(tvec); 
x  -  zeros(rx,N+l); 
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x(:,l)  -  xO; 

B  »  zeros(xO); 


%  Loop  through  and  calculate  new  positions 
oldt  ■  tvec(l); 
for  k  «  1:N 

%  Only  recalculate  phi  and  del  when  necessary 
if  (tvec(k)  oldt)  |  (k  «»  1 ) 

[phi, del]  «  c2d(A,B.tvec(k)); 
end; 

%  Discrete  state  projection 
x(:,k+l)  -  phi*x(:,k); 

%  Save  the  current  time  interval  for  the  check  above 
oldt  *  tvec(k); 
end: 


J.  KF  ERR.M 


function  err  «  kf_err(xe,xt,T) 

% 

%KF_ERR 

% 

%  err  »  kf_err(xe,xt,T)  takes,  the  following  arguments: 
% 

%  xe  vector  of  estimated  target  positions, 

%  xt  vector  of  actual  target  positions, 

%  T  vector  of  Times  of  estimates. 

% 

%  This  function  returns  the  total  weighted  error: 

% 

%  err  «  sum(abs(distance)  *  Time) 

%  Stephen  L.  Spehn  24  Nov  1989 

%  Check  arguments 
if  nargin  —  3 

error(’Incorrect  number  of  arguments  to  KF_ERR.’); 
end; 

%  Compute  the  error 

dif _ ■  sqrt((xt(  1 ,:)  -  xe(l,:)).A2  +  (xt(3,:)  -  xe(3,:)).A2); 

err  =  sum(  dif _ .*  T  ); 
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K.  NOISE.M 


%  This  file  generates  the  suite  of  noise  matrices  for  SENRUN01.M 

.%  Stephen  L.  Spehn  31  Jan  1990 

%  defined  constants 
load  include.mat 

Num_Noise_Runs  *  30;  %  the  more  runs,  the  better  the  average 

Standard_Noise_Seed  »  1989;  %  1  ..  2A31  -  1 

Use_Standard_Noise  -  YES;  %  YES:  same  random  numbers  every  day 

%  Generate  a  set  of  observation  noise  matrices. 
rand(’normal’); 

if  Use_Standard_Noise  YES 

rand(’seed\Standard_Noise__Seed); 
else 

rand(,seed’,sum(clock)*  1 00); 
end; 

for  nl  *  l:Num_Noise_Runs; 

eval([’obs_noise\int2str(nl),’  =  rand(2,TIME_INTERVALS+l );’]); 
end; 

save  noise. mat 
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L.  SUPPORT  FILES 


1.  Errellip.m 


function  [x,y]  *  errellip(M,K,Pr,n,MD) 

% 

%errellip(M,K,Pr,n,MD) 

% 

%  This  function  is  used  to  generate  vectors  of  x  and  y 
%  coordinates  for  the  plotting  of  error  ellipses.  It  takes 
%  the  following  arguments: 

% 

%  M  Mean  Vector  (2x1) 

%  K  Covariance  Matrix  (2  x  2) 

%  Pr  Probability  (0  ..  1 ) 

%  n  Number  of  points  to  compute 

%  MD  Compute  by  Mahalanobis  Distance  vice  probability 
%  0  =  Probability 

%  1  *  Mahalanobis  Distance 

% 

%  and  returns  x  and  y  vectors  of  the  confidence  ellipse  for  the 
%  given  probability  or  Mahalanobis  Distance. 

%  Stephen  L.  Spehn  15  Nov  1989 

%  Check  the  input  arguments 
if  nargin  ~=  5 

error(’Incorrect  number  of  arguments  to  ERRELLIP.’); 
end; 

%  Compute  the  Mahalanobis  Distance  for  the  ellipse 
if  MD  ==  1 
c  =  abs(Pr); 
else 

Pr  *  max(min(.995,Pr),.005); 
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%  Cubic  spline  fit  for  the  ellipse  constant. 

%  Using  this  method  is  a  compromise  between  speed  and  accuracy. 
PP  -  {  12; 


.005;  .01;  .025;  .05;  .1;  .25;  .5;  .75;  .9;  .95;  .975;  .99;  .995; 
4.0; 


4. 190809 197869072e+ 1 ; 
3.970929262330003; 

1 .6288224 1111 034e+ 1 ; 
4. 1 76985755223079e+2; 
-  3.8 1 0356328008524e- 1 ; 
5.444089 169224 1 42e- 1 ; 
2.55841940780766; 
4.6070422925247e+2; 
2.020857475864537; 
2.122852230998054; 
3.862381141104123; 
4.382 1 33265898673e+ 1 ; 


4. 190809 197872625e+l; 
2.15930349191602; 
8.243758565 140 19e+l; 
2.087990965023806e+5; 
2.4758574o879301 le-1; 
1.140048306271903; 
1.4774587491 13522e+l; 
4.9203162241 66436e+2; 
2.02019022643493; 
2.20707509215777; 
8.195632865839841; 
6.76397289507 1458e+l; 


-2.1 18721 29 1999464e+l; 
5.9557937356473 19e- 1 ; 
2.725551 521454689e+3; 
2.087990965023842e+5; 
2.133449885921905; 
2.111734877634124; 
5.1871 50103426589e+l; 
9.887990965023757e+3; 
2.055905760926949; 
2.694842569743673; 

1.81 92546 14465004e+l; 
2.2334006776232 le+2; 


.01;  .0201;  .0506;  .103;  .211;  .575;  1.39;  2.77;  4.61;  5.99;  7.38;  9.21  ]; 


c  »  ppvaI(pp,Pr); 
end; 

%  Get  Eigenvectors  and  Eigenvalues  of  the  Covariance  matrix 
[Evec,Eval]  =  eig(K); 
sigx  =  sqrt(Eval(l,l)); 
sigy  =  sqrt(Eval(2,2)); 


%  Parameterize  the  ellipse  equations  by  the  angle,  t 
t  *  0:(2*pi,/n):(2*pi); 
xv  =  sigx*c*cos(t); 
yv  =  sigy*c*sin(t); 

%  Translate  back  to  the  original  coordinates 
%  and  add  in  the  mean  (center  of  the  ellipse) 
x  *  (xv*Evec(l,l)  +  yv*Evec(l,2)  +  M(l))’; 
y  *  (xv*Evec(2,l)  +  yv*Evec(2,2)  +  M(2))’; 
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2.  Int2str2.m 


function  str  ■  int2str2(i) 

% 

%INT2STR2 

% 

%  str  «  int2str2(i)  returns  a  two  character  string  with  the 
%  value  of  i  as  a  zero  padded  integer  to  two  places.  Examples 
%  are  given  below. 

% 

%  i  «  5  str  *  ’05’ 

%  i  -  50  str  -  ’50’ 

%  i  =  500  str  -  ’500’ 

%  i  «  -5  str  *  ’-5* 

%  Stephen  L.  Spehn  30  Nov  1989 

str  »  int2str(i); 

if  (i  <  10)  &  (i  >»=  0) 
str  «  [’O’, str]; 
end; 


134 


3.  Reset. m 


function  reset 
% 

%RESET 

% 

%  This  function  is  used  to  reset  various  MATLAB  parameters 
%  so  that  the  current  simulation  will  be  unaffected  by  previous 
%  runs. 

%  Stephen  L.  Spehn  25  Nov  1989 

hold  off; 
subplot(lll); 

axisfnormaOiaxis^l  2  3  4]);axis; 

clg.clc; 

clear; 


4.  Rotate  v.m 


function  new_vector  ■  rotate__v(  vector, angle) 

% 

%new_vector  “  rotate_v(vector, angle); 

% 

%  This  function  takes  a  vector  of  x  and  y  coordinates  and  an 
%  angle  of  rotation  specified  in  radians.  It  returns  the 
%  rotated  vector. 

% 

%  vector  [  x 

%  y  ]; 

%  angle  angle  of  rotation  in  radians  (positive  CW) 

% 

%  new_vector  [  x’ 

%  y*  3: 

%  Stephen  L.  Spehn  17  Jan  1990 

%  Check  input  arguments  for  number  and  size 
if  nargin  -■  2 

error(’Incorrect  number  of  arguments  to  ROTATE_V,’); 
end; 

[rv,cv]  =  size(vector); 
if  (rv  2)  |  (cv  1) 

error(Tnput  vector  incorrect  size.’); 
end; 

%  Calculate  transformation  matrix  and  rotate  the  vector 
cosl  *  cos(angle); 
sinl  =  sin(angle); 

T  *  [  cosl  sinl 
-sinl  cosl  ]; 

new_vector  *  T  *  vector; 
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5.  Timestr.m 


function  tstr  ■  timestr 
% 

%TIMESTR 
*  % 

%  This  function  returns  the  time  and  date  in  a  string,  i.e., 
% 

%  1525,  28  Nov  1989 

%  Stephen  L.  Spehn  28  Nov  1989 

months  *  [’  Jan  ’ 

’  Feb  ’ 

’  Mar  ’ 

’  Apr  ’ 

’  May  ’ 

’  Jun  ’ 

1  Jul 1 
’  Aug  • 

’  Sep  ’ 

1  Oct  • 

'  Nov  ' 

1  Dec  ’  ]; 

td  =  fix(clock); 

tstr  =  [int2str2(td(4)),int2str2(td(5)),\ 

int2str(td(3)),months(td(2),:),int2str(td(  1 ))]; 
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